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Active  Vibration  Control  of  a  Flexible  Base  Manipulator 
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257  pages 

Directed  by  Dr.  Wayne  J.  Book 

A  rigid  (micro)  robot  mounted  serially  to  the  tip  of  a  long,  flexible  (macro)  robot 
is  often  used  to  increase  reach  capability,  but  flexibility  in  the  macromanipulator  can 
make  it  susceptible  to  vibration.  A  rigid  manipulator  attached  to  a  flexible  but  unactuated 
base  was  used  to  study  a  scheme  to  achieve  micromanipulator  positioning  combined  with 
vibration  damping  of  the  base.  Inertial  interaction  forces  and  torques  acting  at  the  base  of 
the  rigid  robot  were  studied  to  determine  how  to  use  them  to  damp  the  base  vibration. 

The  ability  of  the  rigid  robot  to  create  inertial  interactions  varies  throughout  the 
workspace.  There  are  also  “inertial  singularity”  configurations  where  the  robot  loses  its 
ability  to  create  interactions  in  one  or  more  degrees  of  freedom.  A  performance  index 
was  developed  to  quantify  this  variation  in  performance  and  can  be  used  to  ensure  the 
robot  operates  in  joint  space  configurations  favorable  for  inertial  damping.  When  the 
performance  index  is  used  along  with  appropriate  vibration  control  feedback  gains,  the 
inertia  effects,  or  those  directly  due  to  accelerating  the  rigid  robot  links,  have  the  greatest 
influence  on  the  interactions.  By  commanding  the  link  accelerations  out  of  phase  with 
the  base  vibration,  energy  will  be  removed  from  the  system.  This  signal  is  then  added  to 
the  rigid  robot  position  control  signal.  Simulated  and  measured  interaction  forces  and 
torques  generated  at  the  base  of  a  rigid  robot  are  compared  to  verify  conclusions  drawn 


about  the  controllable  interactions.  In  addition,  simulated  and  experimental  results 
demonstrate  the  combined  position  control  and  vibration  damping  ability  of  the  scheme. 
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SUMMARY 


A  rigid  (micro)  robot  mounted  serially  to  the  tip  of  a  long,  flexible  (macro)  robot 
is  often  used  to  increase  reach  capability,  but  flexibility  in  the  macromanipulator  can 
make  it  susceptible  to  vibration.  A  rigid  manipulator  attached  to  a  flexible  but  unactuated 
base  was  used  to  study  a  scheme  to  achieve  micromanipulator  positioning  combined  with 
vibration  damping  of  the  base.  Inertial  interaction  forces  and  torques  acting  at  the  base  of 
the  rigid  robot  were  studied  to  determine  how  to  use  them  to  damp  the  base  vibration. 

The  ability  of  the  rigid  robot  to  create  inertial  interactions  varies  throughout  the 
workspace.  There  are  also  “inertial  singularity”  configurations  where  the  robot  loses  its 
ability  to  create  interactions  in  one  or  more  degrees  of  freedom.  A  performance  index 
was  developed  to  quantify  this  variation  in  performance  and  can  be  used  to  ensure  the 
robot  operates  in  joint  space  configurations  favorable  for  inertial  damping.  When  the 
performance  index  is  used  along  with  appropriate  vibration  control  feedback  gains,  the 
inertia  effects,  or  those  directly  due  to  accelerating  the  rigid  robot  links,  have  the  greatest 
influence  on  the  interactions.  By  commanding  the  link  accelerations  out  of  phase  with 
the  base  vibration,  energy  will  be  removed  from  the  system.  This  signal  is  then  added  to 
the  rigid  robot  position  control  signal.  Simulated  and  measured  interaction  forces  and 
torques  generated  at  the  base  of  a  rigid  robot  are  compared  to  verify  conclusions  drawn 
about  the  controllable  interactions.  In  addition,  simulated  and  experimental  results 
demonstrate  the  combined  position  control  and  vibration  damping  ability  of  the  scheme. 
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CHAPTER  I 


INTRODUCTION 


1.1  Motivation 

The  objective  of  this  research  was  to  develop  a  combined  position  and  enhanced 
vibration  control  scheme  for  a  rigid  manipulator  attached  to  a  flexible  base.  The 
configuration  is  similar  to  a  macro/micromanipulator  (Figure  1-1),  which  has  links  that 
are  long  and  lightweight  with  a  rigid  robot  attached  serially  to  the  end.  Macro/micro 
manipulators  are  desirable  for  certain  uses,  because  the  macromanipulator  can  provide 
long  reach  capability  by  moving  the  robot  to  the  general  area  of  interest  where  it  can  then 
be  used  for  fine-tuned  positioning.  These  are  often  used  to  perform  tasks  that  humans 
may  be  incapable  of  doing  or  that  are  dangerous  for  humans. 

One  application  is  in  the  nuclear  industry  where  macro/micromanipulators  are  used  to 
remove  nuclear  waste  from  underground  storage  tanks  [25].  In  the  application  described 
in  the  reference,  a  39  foot,  seven  degree  of  freedom  long-reach  manipulator  was  used 
with  a  rigid  end  effector  to  clean  seven  storage  tanks  at  Oak  Ridge  National  Laboratory 
from  1996-2000.  Two  end  effectors  were  used:  one  type  measured  the  radiation  field, 
while  the  other  scarified  the  tank  walls. 
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Another  growing  application  is  in  space,  where  long  reach  capability  is  needed,  but 
weight  is  crucial  [41,56,77,93].  The  space  shuttle  has  used  a  remote  manipulator  system 
(RMS),  or  Canadarm,  which  was  initially  launched  and  used  in  1981.  One  problem  is 
that  approximately  1/3  of  the  time  spent  by  astronauts  in  operation  of  the  RMS  is  spent 
waiting  for  vibration  to  decay  [41].  The  second-generation  system.  Space  Station 
Remote  Manipulator  System,  or  Canadarm  2,  is  a  seven-jointed  arm  that  was  designed  to 
maneuver  large  payloads  around  the  International  Space  Station.  It  will  sometimes  be 
used  with  the  Special  Purpose  Dexterous  Manipulator  to  provide  specific  maintenance 
tasks. 

The  problem  is  that  flexible  links  are  difficult  to  control  and  are  susceptible  to 

% 

vibration  induced  by  movement  of  the  robot  itself  or  by  external  disturbances.  The  many 
degrees  of  freedom  involved  make  control  of  the  coupled  system  a  complex  task.  This 
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research  considers  the  base  of  the  rigid  robot  to  be  flexible,  where  the  base  motion  is 
similar  to  that  due  to  vibration  at  the  tip  of  a  flexible  macromanipulator  with  locked  joints 
(Figure  1-2).  It  is  assumed  for  this  work  the  joints  of  the  macromanipulator  are  not 
actuating  so  the  only  vibration  in  the  system  is  due  to  externally  applied  disturbances  or 
motion  of  the  rigid  manipulator. 

Many  researchers  have  tackled  the  problem  of  developing  control  schemes  to 
eliminate  unwanted  vibration  in  flexible  manipulators.  One  area  involves  determining 
trajectories  that  will  avoid  or  minimize  inducing  vibration;  however,  these  schemes  are 
not  useful  for  controlling  vibration  once  it  occurs.  The  macromanipulator  actuators  are 
not  a  good  option  for  vibration  damping  due  to  bandwidth  limitations  and  non-collocation 
of  the  actuators  and  end  point  vibration.  This  creates  a  non-minimum  phase  problem  due 
to  time  delays,  further  exacerbated  by  flexibility  in  the  link(s).  In  addition,  since  only 
gross  positioning  capability  is  really  needed  for  the  macromanipulator,  it  is  an 
unnecessary  increase  in  cost  and  system  complexity  to  use  its  actuators  for  vibration 
control  in  addition  to  their  already  difficult  task. 

The  use  of  the  rigid  manipulator  to  damp  vibration  in  the  macromanipulator  has 
proven  to  be  a  promising  area.  The  micromanipulator  produces  inertial  forces  and 
torques  that  act  as  disturbances  to  the  macromanipulator  under  decoupled  control.  Under 
coupled  control,  these  inertial  effects  can  be  used  as  damping  forces  and  torques  and 
applied  directly  to  the  tip  of  the  macromanipulator.  This  also  makes  the  system 
minimum  phase,  further  reducing  the  complexity  of  the  control  task.  In  addition,  it  is 
much  easier  to  provide  high  bandwidth  actuators  for  a  small  robot  arm  than  for  a  large 
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one,  so  its  actuators  can  respond  more  quickly  and  efficiently  to  provide  large  inertial 
forces  and  torques.  Previous  methods  of  damping  vibration  in  this  manner  include 
energy  dissipation  methods  and  inertial  damping  methods.  The  goal  here  is  to  command 
the  rigid  robot  to  act  as  an  active  vibration  damper,  damping  the  motion  of  the 
macromanipulator  at  the  natural  frequencies  of  the  system.  These  along  with  other 
methods  of  controlling  macro/micromanipulators  are  discussed  in  more  detail  in  Chapter 
2. 


1 .2  Problem  Overview 

In  this  work,  the  rigid  robot  control  scheme  must  perform  the  dual  task  of  damping 
unwanted  base  vibration  (macromanipulator  vibration)  while  providing  position  control 
of  the  rigid  robot.  On  the  one  hand,  if  the  motion  of  the  micromanipulator  or  combined 
system  is  completely  prescribed  by  the  task,  this  method  is  not  useful.  However,  under 
circumstances  where  the  task  will  allow  small  movements  of  the  rigid  robot  to  damp  the 
vibration,  this  technique  can  be  very  effective.  After  all,  if  the  system  is  vibrating 
uncontrollably  the  system  performance  is  impacted.  The  controlled  interactions  are 
collocated  with  the  vibration  at  the  tip  of  the  macromanipulator,  and  the  rigid  robot  can 
respond  quickly  to  create  the  inertial  damping  forces  and  torques.  The  goal  here  is  to 
reduce  the  vibration  as  quickly  and  efficiently  as  possible  so  the  system  can  continue  with 
its  task.  This  method  requires  no  hardware  modifications  other  than  some  type  of 
measurement  of  the  vibration. 
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Most  of  the  literature  addresses  macro/micromanipulator  position  control  or  vibration 
control  alone,  but  few  researchers  address  both.  The  authors  that  have  addressed  both 
assume  limited  base  flexibility,  thereby  limiting  the  applicability  of  the  work.  In 
addition,  simulations  and  hardware  demonstrations  have  been  limited  mostly  to  planar 
translational  vibration.  Finally,  operation  throughout  the  workspace  has  not  been 
addressed,  in  particular  at  locations  where  coupling  effects  between  the  macro  and 
micromanipulator  are  unsuited  to  vibration  damping. 

The  control  scheme  described  in  this  dissertation  was  tested  in  simulations  and 
experiments  in  two  main  scenarios.  The  first  was  with  the  robot  operating  at  a  desired 
joint  space  configuration  and  tested  its  ability  to  damp  vibration  induced  by  an  applied 
disturbance.  The  second  scenario  was  for  point-to-point  motion  where  the  rigid  robot  is 
moving  from  an  initial  to  a  final  joint  space  configuration  in  a  given  time  period.  Both  of 
these  allow  flexibility  in  choosing  between  alternate  inverse  kinematic  joint  space 
configurations.  If  the  joint  space  configuration  of  the  rigid  robot  is  a  prescribed  part  of 
the  control  scheme,  another  method  of  damping  may  be  required. 

1.3  Contributions 

The  contributions  of  this  thesis  are: 

1 .  Extension  of  the  macro/micromanipulator  control  problem  to  multiple  degrees  of 
freedom  by  considering  the  analogous  problem  of  a  rigid  manipulator  mounted  on  a 
flexible  base. 
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2.  Investigation  of  inertial  singularities  and  variation  in  inertial  damping  performance 
throughout  the  workspace. 

3.  Development  of  a  control  scheme  that  provides  active  base  vibration  damping  in 
parallel  with  rigid  robot  position  control  and  establishment  of  appropriate  vibration 
control  gain  limits. 

4.  Verification  of  the  above  via  simulation. 

5.  Experimental  work  including  verification  of  the  accuracy  of  the  interaction  force 
and  torque  predictions  and  demonstration  of  the  effectiveness  of  the  control  scheme  on  a 
realistic  multi-degree  of  freedom  testbed. 

1.4  Organization  and  Overview 

This  dissertation  is  organized  in  the  following  manner: 

Chapter  1  discusses  the  motivation  of  the  research,  contributions  of  the  work,  and 
outlines  the  dissertation. 

Chapter  2  reviews  the  current  state  of  literature  on  the  subject  of 
macro/micromanipulator  control  and  limitations  of  previous  research. 

Chapter  3  describes  modeling  of  the  flexible  base  manipulator.  A  Lagrangian 
approach  with  a  finite  number  of  assumed  modes  was  used  to  represent  the  flexible 
manipulator,  while  a  recursive  Newton-Euler  formulation  was  used  to  derive  expressions 
for  the  interaction  forces  and  torques  acting  between  the  macro  and  micromanipulator. 
The  general  form  of  these  interactions  is: 
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F!F=Bf(d)d+Nf(eiej,d)  +  Gf(d)  +  Cf(d)q  +  Nfc(q,q,e,6) 

7  ^  0  +  A^r  0 «?  (.<9  ,  (9 )  +  Or0  )  +  Cr0  «?)ci  +  A^r0c  (q,  q,  <?  )  (1-1) 

where  0  represents  the  rigid  robot  joint  variables  and  q  represents  the  flexible 
manipulator  generalized  coordinates.  The  rigid  robot  configuration,  9,  joint  velocities 
and  accelerations,  and  flexible  base  velocities  and  accelerations  drive  the  interactions. 
The  goal  was  to  study  these  interactions  in  order  to  determine  how  to  use  them  in  the 
control  scheme  to  damp  the  macromanipulator  vibration. 

Chapter  4  discusses  in  more  detail  the  controllable  interactions,  or  the  first  two  terms 
in  each  equation  in  1-1.  A  performance  measure  is  introduced  which  predicts  the 
effectiveness  of  the  rigid  robot  in  creating  these  interactions.  The  rigid  inertia  effects 
(Bf(0)  and  Bxo(0))  are  particularly  important  for  two  reasons.  First,  the  rigid  robot  must 
have  enough  inertia  to  effectively  apply  interaction  forces  and  torques  to  the 
macromanipulator.  The  ratio  of  the  rigid  inertia  to  flexible  inertia  effects  becomes  an 
important  part  of  the  performance  index,  discussed  in  Chapter  5.  Second,  there  are  joint 
workspace  configurations  where  these  matrices  become  singular.  These  “inertial 
singularities”  represent  physical  limitations  in  that  an  inertia  force  or  torque  cannot  be 
created  in  one  or  more  degrees  of  freedom.  The  variation  in  performance  is  driven  by  the 
joint  space  configuration  of  the  rigid  robot,  so  the  performance  measure  can  be  used  to 
choose  joint  space  configurations  better  suited  for  inertial  damping.  The  inertia  effects 
dominate  the  interactions  in  most  non-singular  configurations.  However,  the  nonlinear 
rigid  effects  may  also  become  significant  in  certain  cases  and  these  are  also  discussed 
here. 
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The  control  scheme  is  discussed  in  Chapter  5.  The  overall  schematic  is  shown  in 


Figure  1-3.  The  flexible  base  vibration  controller  takes  the  form: 

t  =  -ID(6)*Kx  (1-2) 

where  ID(0)  represents  an  inverse  dynamics  function  [4,19]  designed  to  cancel  the 
significant  rigid  robot  dynamics  and  x  represents  the  motion  of  the  flexible  base.  The 
rigid  robot  motion  will  be  commanded  to  absorb  the  vibration  energy  of  the  flexible  base. 


Overview  of  Control  Scheme 


Chapter  6  discusses  simulations  of  vibration  damping  of  a  three  degree  of  freedom 
anthropomorphic  robot  mounted  on  a  flexible  base.  Simulations  demonstrating 
disturbance  rejection  as  well  as  the  use  of  the  performance  index  in  predicting  better  joint 
space  configurations  for  vibration  damping  during  commanded  motion  are  presented. 
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Chapter  7  discusses  the  experimental  testbed  and  presents  results  from  two  areas  of 
testing.  First,  predicted  and  measured  interaction  forces  and  torques  generated  at  the  base 
of  a  rigid  three  degree  of  freedom  anthropomorphic  robot  are  presented,  which  verified 
many  of  the  results  presented  in  Chapter  4.  Second,  the  ability  of  the  controller  to  damp 
vibration  on  a  multi-degree  of  freedom  testbed  was  tested.  The  macromanipulator 
consists  of  two  flexible  links  in  a  fixed  joint  configuration.  Three  links  of  a  six  degree  of 
freedom  micromanipulator  were  used  for  vibration  damping.  Some  promising  results  are 
presented  demonstrating  overall  vibration  energy  from  the  system.  However,  several 
implementation  issues  arose  that  limited  the  effectiveness  of  the  scheme  on  the  testbed. 
These  are  discussed  in  more  detail  as  well  as  proposed  means  of  addressing  these  issues 
for  future  work. 

Chapter  8  summarizes  the  results  and  suggests  area  of  further  research.  Finally, 
Appendix  A  includes  the  equations  for  the  interaction  forces  and  torques  for  four  typical 
robot  configurations. 
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CHAPTER  II 


LITERATURE  REVIEW 

2.1  Introduction 

This  chapter  reviews  the  general  topic  of  macro/micromanipulator  control. 
Macro/micromanipulators  were  introduced  in  the  early  1980s  as  a  means  of  improving 
endpoint  control  of  flexible  manipulators,  which  were  becoming  more  common  for 
applications  where  long  reach  capability  was  needed.  Positioning  errors  due  to  flexibility 
and  other  inaccuracies  in  the  links  of  the  macromanipulator  are  compensated  for  by  the 
micromanipulator. 

First,  literature  on  modeling  flexible  and  rigid  manipulators  is  reviewed.  Since  this  in 
itself  is  a  very  broad  area,  the  research  discussed  here  is  specifically  relevant  to  modeling 
combined  flexible/rigid  systems.  Next,  general  methods  of  controlling 
macro/micromanipulators  are  discussed.  The  problem  that  becomes  quickly  apparent  is 
the  large  number  of  degrees  of  freedom  involved  and  complexity  of  the  resulting  control 
problem.  The  links  of  the  macromanipulator  are  susceptible  to  vibration,  so  there  are 
additional  degrees  of  freedom  that  need  to  be  controlled  as  well  as  the  rigid  coordinates. 
One  option  to  reduce  the  complexity  of  the  problem  is  to  decouple  control  of  the  rigid 
and  flexible  robots.  In  this  case,  the  macromanipulator  provides  gross  positioning  while 
the  rigid  robot  provides  the  fine-tuned  positioning.  However,  there  are  also  problems 
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with  this  technique.  The  rigid  robot  produces  inertial  forces  and  torques  which  can  act  as 
disturbances  to  the  macromanipulator  and  worsen  the  vibration  problem.  In  addition, 
with  larger  and  more  flexible  macromanipulators,  vibration  amplitudes  can  become  too 
large  for  the  rigid  robot  to  compensate  for.  Thus  another  area  of  research  evolved  which 
focused  on  methods  of  commanding  the  micromanipulator  to  reduce  vibration  in  the 
macromanipulator.  One  way  is  to  command  the  rigid  robot  using  trajectories  that  will 
reduce  these  disturbances.  An  alternate  approach  is  to  use  the  disturbances  to  damp  the 
vibration,  which  is  the  basis  of  the  work  described  in  this  dissertation.  The  current  state 
of  research  in  this  area  is  reviewed  as  well  as  limitations  of  the  work  performed  thus  far. 

In  addition,  there  has  been  a  great  deal  of  related  research  in  the  area  of  space 
robotics.  This  area  has  not  been  widely  recognized  as  being  related  to  the  problem  of 
earth-based  macro/micromanipulator  control.  However,  the  approach  taken  in  this  thesis, 
where  the  rigid  robot  is  considered  attached  to  a  flexible  base,  is  very  similar  to  space 
robotics  research  that  considers  a  rigid  robot  mounted  to  a  floating  base  (spacecraft). 
Some  of  the  applicable  space  robotics  research  is  also  reviewed  here. 

2.2  Flexible  and  Rigid  Manipulator  Modeling 

2,2.1  Flexible  Robot  Modeling 

There  are  many  methods  available  to  model  flexible  link  robots.  Since  the  links  are 
distributed  parameter  systems,  their  motion  is  described  by  partial  differential  equations 
instead  of  ordinary  differential  equations  and  hence  modeling  can  become  very 
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challenging.  In  addition  to  the  nonlinear  rigid  dynamics  commonly  found  in  robotic 
systems,  flexible  manipulators  also  exhibit  elastic  behavior. 

Book  [8]  developed  a  recursive  Lagrangian  approach  for  modeling  flexible  link 
robots.  Describing  the  position  of  a  point  on  a  flexible  link  requires  both  rigid  and  elastic 
coordinates,  so  he  suggested  the  use  of  4  x  4  transformation  matrices  for  more  compact 
representation.  By  choosing  a  finite  number  of  assumed  modes  to  model  the  elastic 
motion,  the  position  of  a  point  along  each  flexible  link  can  be  written  in  terms  of  the  rigid 
and  flexible  coordinates.  Expressions  for  kinetic  and  potential  energy  of  the  system  can 
then  be  developed.  The  kinetic  energy  terms  consist  of  translational  and  rotational 
energy  of  each  link.  Potential  energy  terms  consist  of  elastic  bending,  gravity,  and 
shearing  deformation  effects. 

Several  authors  have  considered  the  relative  importance  of  the  energy  terms  and 
under  what  circumstances  certain  effects  may  be  neglected  [46,52,72,83].  Most  authors 
involved  in  modeling  flexible  manipulators  assume  Euler-Bemoulli  bending  theory 
applies.  When  this  is  the  case,  rotational  inertia  terms  may  be  assumed  negligible,  and 
potential  energy  terms  only  need  to  include  elastic  bending  and  gravity  effects  [83].  The 
resultant  equations  are  integrated  over  the  spatial  variable  and  used  with  Lagrange’s 
equations  to  derive  the  equations  of  motion.  One  advantage  of  this  method  is  that  the 
energy  terms  can  include  as  much  or  as  little  detail  as  needed.  Modal  damping  may  also 
be  added  if  desired.  Regardless  of  the  method  used  to  derive  them,  the  equations  of 
motion  take  the  form: 
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Mb  (q)q  +  Cb(q,q)  +  Kbq  +  Gb  (q)  -  Q 

Mb=  inertia  matrix 

C =  nonlinear,  coupling,  and  viscoelastic  effects 
Kb  =  stiffness  matrix 
Gb  =  gravitational  effects 
Q  =  generalized  forces 

flexible  coordinates 
rigid  joint  coordinates 


(2-1) 


The  generalized  forces  are  the  joint  actuation  torques  and  contact  forces  and  torques.  In 
the  case  of  a  macro/micromanipulator,  the  contact  forces  and  torques  will  be  those 
created  by  the  micromanipulator. 

Lee  [32],  Lew  [34],  and  Obergfell  [53]  used  this  method  to  model  a  two-link  flexible 
robot  at  Georgia  Tech  called  RALF  (Robot  Arm,  Large  and  Flexible).  Other  examples  of 
modeling  a  single  flexible  link  can  be  found  in  Cannon  [15],  Loper  [42],  Nataraj  [52], 
and  Smart  and  Wiens  [72]. 

2.2.2  Rigid  Robot  Modeling 

Common  methods  of  modeling  rigid  robots  are  a  Lagrangian  approach  or  a  Newton- 
Euler  formulation.  Detailed  descriptions  of  both  methods  can  be  found  in  Craig  [18], 
McKerrow  [48],  or  Sciavicco  and  Siciliano  [61].  The  equations  of  motion  take  the  form: 
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BT(O)0  +  Nr(0,0)O  +  GT(O)  =  T 

0  =  rigid  joint  coordinates 

Bv(0)  =  symmetric,  positive  definite  inertia  matrix 

Nr(0,0)  =  centrifugal  and  coriolis  effects 

G  r  (0  )=gravitational  effects 

r  =  joint  actuation  torques  (2-2) 


2.2.3  Coupled  Macro/Micromanipulator  Models 

One  method  of  modeling  macro/micromanipulators  is  using  the  recursive  Lagrangian 
approach  mentioned  above  and  treating  the  last  few  links  as  rigid.  However,  this  quickly 
becomes  long  and  tedious.  Lew  [34]  developed  a  more  efficient  method  of  deriving  the 
equations  of  motion  of  two  robots  connected  serially.  He  concentrated  on  identifying  the 
coupling  dynamics  between  the  two  manipulators  assuming  known  models  for  each.  The 
coupled  equations  of  motion  take  the  form: 


~Mt+MuXq,e)  MJq,e) 

V 

+ 

+ 

X  o“ 

V 

'o' 

Ml(q,6)  B,(«)  _ 

0 

_NT(0,0)+Cb/r( 4q,M)_ 

.0  0 

0 

T 

(2-3) 


The  notation  used  here  is  slightly  different  from  the  notation  used  in  [34]  and  his  papers 
[35-40]  in  order  to  be  consistent  with  the  notation  used  in  equations  2-1  and  2-2.  Mb,  Q>, 
and  Kb  represent  macromanipulator  properties  and  are  defined  along  with  q  in  equation 
2-1.  Bt  and  NT  represent  the  rigid  robot  dynamics  and  are  defined  along  with  0  in 
equation  2-2.  Mb/r  and  Mbr  are  coupling  inertia  matrices,  and  Q,r  and  Cb/r  are  nonlinear 
coupling  terms  (gravitational  effects  are  included  in  the  nonlinear  terms).  It  is  further 
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assumed  the  macromanipulator  is  not  actuating  so  the  only  joint  torques  and  rigid 
coordinates  that  vary  are  those  associated  with  the  rigid  robot.  Most  of  the 
macro/micromanipulator  control  literature  uses  this  form  of  the  coupled  equations  of 
motion  as  a  baseline  model. 

Sharf  [63]  introduced  a  means  of  effectively  decoupling  the  macro  and 
micromanipulator  models  by  finding  expressions  for  the  reaction  forces  acting  between 
the  two  bodies.  This  is  equivalent  to  equation  2-3,  the  only  difference  being  the  explicit 
definition  of  the  interactions. 

2.3  General  Macro/Micromanipulator  Control  Approaches 

The  control  of  flexible  manipulators  has  been  studied  extensively.  Control  of  the 
distributed,  nonlinear  systems  is  difficult  and  researchers  have  examined  end  point 
sensing,  robust  control,  vibration  suppression,  and  command  shaping  techniques,  among 
others,  to  better  control  them.  Book  [9,10]  discussed  many  of  the  problems  associated 
with  controlling  flexible  manipulators.  As  discussed  in  section  2.2.1,  modeling  is 
difficult  but  achievable  if  the  system  modes  of  interest  can  be  limited  to  a  finite  number 
of  modes.  The  control  problem  is  extremely  complicated  for  many  reasons.  First, 
flexible  manipulators  are  susceptible  to  vibration,  either  induced  by  movement  of  their 
long,  flexible  links  or  external  disturbances.  Second,  the  number  of  control  variables 
(joint  variables)  is  less  than  the  number  of  mechanical  degrees  of  freedom,  which  include 
both  the  rigid  and  flexible  coordinates.  Third,  the  dominant  closed  loop  poles  of  the 
system  do  not  become  more  stable  with  increasing  position  control  feedback  gains.  This 
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limits  the  achievable  bandwidth  to  about  1/3  that  of  a  rigid  manipulator  [10].  Thus,  lower 
bandwidth  actuators  are  typically  used  and  they  may  not  be  fast  enough  to  respond  to  the 
vibration.  In  addition,  the  actuators  are  located  at  the  joints  of  the  macromanipulator 
while  the  vibration  of  concern  is  at  the  end  point.  This  non-collocation  issue  further 
complicates  the  control  problem.  Non-minimum  phase  dynamics  can  result  and, 
combined  with  the  many  other  control  issues  associated  with  flexible  manipulators,  may 
threaten  system  stability. 

Sharon,  Hogan,  and  Hardt  introduced  macro/micromanipulators  in  the  early  1980s  as 
a  means  of  improving  endpoint  control  of  flexible  manipulators  [64,65].  They  showed 
that  a  rigid  robot  mounted  serially  to  a  flexible  manipulator  could  be  used  to  compensate 
for  position  errors  caused  by  macromanipulator  flexibility  and  other  inaccuracies.  The 
end  point  position  control  bandwidth  was  chosen  to  be  approximately  15  times  the  first 
natural  frequency  of  the  macromanipulator.  Since  the  micromanipulator  inertia  is 
relatively  small,  it  can  respond  quickly  to  the  rapid  transients  of  the  macromanipulator 
vibration. 

Much  of  the  research  in  the  control  of  macro/micromanipulators  involves  designing 
specialized  coupled  control  schemes.  The  many  degrees  of  freedom  involved  combined 
with  additional  challenges  associated  with  controlling  the  flexible  links  make  coupled 
control  a  difficult  task.  These  control  schemes  fall  into  three  general  categories.  First  are 
schemes  where  both  the  macromanipulator  and  micromanipulator  are  controlled 
concurrently.  The  complexity  of  these  schemes  makes  them  difficult  to  implement  but 
may  be  the  only  solution  in  some  cases.  The  second  area  involves  decoupled 
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macromanipulator  control  schemes  designed  to  control  or  reduce  the  vibration.  Since 
flexible  manipulators  have  been  in  use  for  many  years,  there  is  a  large  pool  of  applicable 
research  that  can  help  reduce  vibration,  including  input  shaping.  A  third  area  considers 
the  decoupled  micromanipulator  controller.  These  include  schemes  that  use  the  rigid 
robot  to  compensate  for  macromanipulator  position  error  as  well  as  schemes  that  actively 
use  the  micromanipulator  to  reduce  the  vibration.  The  latter  is  the  basis  for  this  research 
and  the  background  in  this  area  is  described  in  more  detail  in  the  next  section. 

One  well-known  method  of  improving  trajectory  tracking  is  to  use  an  inverse 
dynamics  function  to  cancel  undesirable  system  dynamics.  Bayo  and  Moulin  [4]  and 
Devasia  and  Bayo  [19]  considered  the  control  of  flexible  manipulators  through  the 
solution  of  the  system  inverse  dynamics.  However,  one  major  problem  with  applying 
this  method  to  a  flexible  link  robot  is  that  it  is  a  non-minimum  phase  system.  When  the 
dynamics  are  inverted  the  inverse  dynamics  model  contains  both  positive  and  negative 
real  eigenvalues. 

Kwon  and  Book  [31]  investigated  inverse  dynamic  trajectory  tracking  for  a  single¬ 
link  flexible  manipulator.  Their  goal  was  to  develop  a  time  domain  inverse  dynamics 
method  that  enabled  a  flexible  manipulator  to  follow  a  given  end  point  trajectory 
accurately  without  overshoot  or  residual  vibration.  They  first  modeled  the  manipulator 
using  the  assumed  modes  method  described  in  section  2.2.1  and  developed  the  inverse 
dynamics  model.  The  tracking  controller  combined  an  inverse  dynamics  function  for 
feedforward  control  with  a  joint  feedback  controller.  They  worked  around  the  non¬ 
minimum  phase  issue  by  extending  the  solution  set  to  include  a  non-causal  solution  and 
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split  the  inverse  dynamics  into  causal  and  anticausal  parts.  They  showed  in  simulation 
and  experiment  the  effectiveness  of  the  controller  in  producing  fast,  vibration-free  motion 
of  a  single  flexible  link  manipulator.  This  work  provided  a  valuable  contribution  in 
showing  that,  with  an  understanding  of  the  unique  problems  associated  with  flexible 
systems,  inverse  dynamics  could  be  implemented  on  them. 

Several  researchers  have  considered  ways  to  reduce  the  complexity  of  combined 
macro/micromanipulator  control  schemes.  Singh  and  Schy  [69]  used  a  control  law  that 
decouples  the  rigid  and  elastic  behavior.  They  considered  a  PUMA-type  robot  with  three 
rotational  joints  mounted  on  a  space  vehicle,  where  the  first  two  links  are  rigid  and  the 
last  link  flexible.  The  elastic  dynamics  are  further  decomposed  into  two  subsystems 
modeling  the  transverse  vibration  of  the  elastic  link  in  two  orthogonal  planes.  A 
proportional-integral-derivative  (PID)  controller  is  used  on  the  joint  angle  errors.  Two 
fictitious  forces  acting  at  the  tip  of  the  flexible  link  are  used  to  damp  the  elastic 
oscillations.  The  scheme  was  shown  effective  and  robust  to  modeling  errors  in 
simulation.  However,  practical  implementation  would  require  a  realistic  way  to  provide 
the  elastic  control  forces. 

Lew  considered  a  different  strategy  of  bracing  the  macromanipulator  [34],  He 
developed  a  hybrid  controller  for  flexible  link  manipulators  that  make  contact  with  the 
environment  at  more  than  one  point  and  proved  its  stability.  He  was  able  to  show 
effective  position  and  force  tracking  control.  Experimental  work  was  performed  at 
Georgia  Tech  with  a  rigid  robot  mounted  on  the  end  of  a  two  link  flexible  manipulator 
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and  demonstrated  that  the  use  of  this  technique  could  effectively  reduce  vibration  in  the 
macromanipulator  in  the  planar  case. 

Yim  and  Singh  [90]  used  an  inverse  control  law  combined  with  a  predictive  control 
law  for  a  macro/micromanipulator.  The  inverse  control  law  was  used  for  end  point 
trajectory  control  of  the  rigid  micromanipulator  and  is  based  on  the  inversion  of  the 
input/output  map.  The  predictive  controller  was  used  for  end  point  control  of  the  flexible 
macromanipulator.  It  was  developed  for  precise  trajectory  tracking  and  designed  so  the 
flexible  dynamics  remained  stable.  The  controller  was  derived  by  minimizing  a  quadratic 
function  of  the  tracking  error,  elastic  deflection,  and  input  control  torques.  The  stability 
of  the  scheme  was  proven  and  its  effectiveness  demonstrated  via  simulations.  They  also 
considered  the  same  predictive  control  law  [89,91]  except  with  a  sliding  mode  controller 
for  the  micromanipulator.  The  micromanipulator  control  scheme  was  variable  structure 
control,  which  is  more  insensitive  to  modeling  errors.  The  sliding  mode  controller  was 
developed  with  the  sliding  surface  functions  of  tip  position,  its  derivative,  and  the  integral 
of  the  tracking  error.  Its  purpose  was  to  ensure  precise  trajectory  tracking  of  the  end 
effector.  Simulations  of  a  single  flexible  link  with  a  two  rigid  link  micromanipulator 
indicated  that  good  end  point  trajectory  control  and  elastic  mode  stabilization  is 
achievable. 

Another  wide  area  of  research  involves  input  shaping  or  trajectory  modification 
techniques  to  avoid  inducing  vibration  during  commanded  moves.  These  techniques 
reduce  vibration  in  a  system  by  convolving  an  impulse  sequence  with  the  desired 
command.  When  the  impulse  sequence  is  chosen  properly,  the  resulting  reduction  in 
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vibration  can  be  drastic,  even  in  the  presence  of  modeling  uncertainties.  The  only 
information  required  is  the  basic  properties  of  natural  frequencies  and  damping  ratios  of 
the  modes  of  concern. 

Singer  and  Seering  [68]  reshaped  an  impulse  input  into  two  impulses,  where  the 
second  was  delayed  by  Vi  the  period  of  vibration  to  be  avoided.  The  shaper  was  placed 
outside  the  feedback  loop.  When  more  than  two  pulses  are  used,  sensitivity  to  modeling 
errors  is  reduced.  The  effectiveness  of  the  technique  was  evaluated  on  a  Space  Shuttle 
Remote  Manipulator  System  (RMS)  simulator  at  Draper  Laboratory  and  showed  a  factor 
of  25  reduction  in  endpoint  residual  vibration  for  typical  moves  of  the  RMS.  Baneijee 
[3]  also  showed  the  effectiveness  of  input  shaping  techniques  on  a  shuttle  experiment 
with  a  very  flexible  payload.  Simulated  spin  up  and  slew  motion  of  the  shuttle  with  two 
150-meter  long  flexible  antenna  booms  indicated  much  less  residual  vibration  in  the 
flexible  antennas  when  the  motion  was  commanded  with  a  three  impulse  shaper. 

Singhose,  Singer,  and  Seering  showed  that  input  shaping  leads  to  much  better 
performance  than  other  filtering  techniques  (Butterworth,  notch,  etc.)  [70].  In  particular, 
they  compared  the  impulse  sequence  length,  residual  vibration,  and  robustness  to 
uncertainties  in  the  system  model.  Each  method  was  used  to  shape  a  step  command 
given  to  a  harmonic  oscillator.  The  results  clearly  indicated  the  input  shapers  are 
significantly  shorter,  yield  considerably  less  vibration,  and  are  far  more  insensitive  to 
modeling  errors  than  the  filters.  Singhose  and  Singer  [71]  also  showed  that  the  use  of 
input  shapers  does  not  significantly  affect  trajectory  tracking.  These  techniques  could  be 
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applied  to  the  macromanipulator  to  reduce  vibration  created  by  its  motion,  or  applied  to 
the  micromanipulator,  or  both. 

Command  shaping  is  a  closely  related  area  except  typically  refers  to  a  scheme  that 
has  a  variable  delay  between  pulses.  This  is  important  for  flexible  manipulators  because 
the  natural  frequencies  of  the  system  and  required  delay  between  pulses  vary  with 
workspace  location.  Magee  and  Book  [45]  applied  command  shaping  techniques  to 
reduce  vibration  induced  by  the  motion  of  a  rigid  robot  mounted  on  a  flexible  base.  They 
used  a  finite  impulse  response  filter  on  the  micromanipulator  joint  position  error.  A 
general  three  term  filter  was  developed  that  can  produce  both  positive  and  negative  filter 
coefficients  depending  on  the  delay  time  value.  Experimental  work  was  performed  using 
a  small  articulated  robot  attached  to  a  much  larger,  flexible  robot.  The  smaller  robot  was 
commanded  to  move  under  proportional-derivative  (PD)  control  with  and  without  the 
filter.  The  use  of  the  filtering  method  resulted  in  a  vibration  amplitude  reduction  of 
nearly  60%.  Input  and  command  shaping  techniques  can  be  very  useful  for  reducing 
vibration  created  by  commanded  movement  of  the  robot.  However,  they  require 
information  about  the  system  and  only  help  in  the  case  of  vibration  induced  by  the 
manipulator  itself.  Vibration  caused  by  external  disturbances  remains  unchecked. 

Xie,  Kalaycioglu,  and  Patel  [87]  developed  an  algorithm  to  command  the  correct 
macromanipulator  actuator  pulses  at  the  end  of  a  maneuver  to  cancel  observed  vibration. 
This  algorithm  was  designed  specifically  for  the  Space  Shuttle  Remote  Manipulator 
System  (RMS).  As  noted  in  Chapter  1,  the  RMS  is  a  realistic  example  of  a  flexible 
manipulator  and  moving  it  tends  to  induce  vibration.  The  technique  described  in  this 
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paper  is  termed  “pulse  active  damping.”  The  concept  is  to  excite  vibration  exactly 
opposite  the  observed  vibration  so  cancellation  is  achieved.  This  concept  is  similar  to 
input  shaping  except  that  it  is  applied  to  the  system  once  the  vibration  is  initiated  and 
measured.  Real-time  system  identification  is  then  performed  to  acquire  the  natural 
frequencies  and  damping  ratios  of  the  system.  The  desired  joint  torque  needed  at  the 
shoulder  to  cancel  the  oscillations  is  applied  at  half  the  natural  frequency  of  the  vibration 
(180°  out  of  phase),  thereby  canceling  it.  Although  this  technique  was  shown  to  be 
effective  in  simulation,  the  large  inertia  of  the  flexible  arms,  non-collocation  of  the 
macromanipulator  actuators  and  the  end  point,  and  limited  actuator  bandwidths  could 
make  it  challenging  to  implement. 

Other  researchers  studied  the  use  of  the  micromanipulator  to  compensate  for 
displacement  errors  caused  by  the  macromanipulator  flexibility.  Ballhaus  and  Rock  [2] 
developed  a  scheme  where  the  macromanipulator  would  move  the  rigid  robot  within 
range  of  the  desired  end  point  position.  If  the  desired  relative  tip  position  was  within 
reach,  a  low  gain  PD  controller  was  used  to  command  the  final  micromanipulator 
position.  If  not,  the  rigid  robot  was  set  to  a  nominal  position.  Experimental  work  on  two 
.52-meter  flexible  links  with  a  two  degree  of  freedom  rigid  micromanipulator 
demonstrated  the  effectiveness  of  this  technique.  The  low  endpoint  gains  ensured  low 
interactions.  However,  the  authors  also  noted  that  with  increasing  gains  the  interaction 
forces  increase  and  can  lead  to  instability. 

Yoshikawa,  Hosoda,  Doi,  and  Murakami  [94]  developed  an  endpoint  tracking  control 
algorithm  that  consists  of  a  PD  macromanipulator  controller  for  global  positioning 
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combined  with  a  dynamic  trajectory  tracking  control  law  for  the  micromanipulator.  The 
micromanipulator  control  scheme  was  designed  to  account  for  the  dynamics  of  the 
system  with  a  nonlinear  state  compensator,  which  linearizes  the  closed  loop  system.  The 
ability  of  the  method  to  achieve  precise  positioning  was  demonstrated  on  a  small-scale 
laboratory  setup  consisting  of  a  macromanipulator  with  two  flexible  links  and  a  two  link 
rigid  robot  for  the  micromanipulator.  However,  with  larger  and  more  flexible 
macromanipulators  with  larger  amplitudes  of  vibration,  these  techniques  become  less 
effective.  In  addition,  the  base  vibration  remains  uncontrolled. 

2.4  Control  and  Coupling  of  Free-Flving  Space  Robots 

Another  wide  area  of  study  that  has  some  applicability  to  macro/micromanipulator 
control  has  been  in  the  analysis  and  control  of  free-flying  space  robots.  Although  some 
principles  are  different  since  space  robots  do  not  have  a  fixed  inertial  base,  some  aspects 
of  this  research  can  be  applied.  Some  of  these  concepts  have  already  begun  to  be  applied 
to  some  of  the  control  techniques  discussed  in  section  2.5. 

Much  work  has  been  done  to  understand  the  dynamic  interactions  between  a  robot 
and  a  free-floating  base.  Torres,  Dubowsky,  and  Pisoni  [77,78]  developed  a  “coupling 
map”  as  an  analytical  tool  to  describe  dynamic  interaction  between  a  space  manipulator 
and  its  base.  The  coupling  map  was  formed  from  the  translational  inertia  of  the  coupled 
macro/micromanipulator  weighted  by  the  stiffness  of  the  macromanipulator.  This 
provides  a  measure  of  the  inertia  forces  acting  between  the  two  bodies  and  a  measure  of 
the  strain  energy  of  the  flexible  system.  It  could  then  be  used  to  find  paths  of  low  energy 


23 


coupling  that  would  result  in  little  interference  between  the  robot  and  its  base,  or  “hot 
spots”  where  the  degree  of  coupling  is  large.  Its  effectiveness  was  evaluated 
experimentally  on  a  three  link  planar  manipulator  mounted  to  a  flexible  beam. 

Xu  and  Shum  [88]  proposed  a  coupling  factor  to  characterize  the  degree  of  dynamic 
coupling  between  a  spacecraft  and  a  robot  mounted  to  it.  The  goal  was  to  use  this  factor 
to  find  robot  motions  that  minimize  the  interference  to  the  spacecraft.  Jiang  [28] 
proposed  a  dynamic  compensability  measure  and  dynamic  compensability  ellipsoid  to 
quantify  the  degree  of  coupling  between  a  robot  and  a  flexible  space  structure.  The 
compensability  measure  predicted  the  ability  of  the  robot  to  compensate  for  the  end- 
effector  position  error  resulting  from  the  flexible  displacements.  This  measure  was  used 
to  find  the  additional  joint  motion  that  would  compensate  for  the  end  effector  error. 

Papadopoulos  and  Dubowsky  [56]  discussed  the  problem  of  “dynamic  singularities” 
in  free-floating  space  manipulators.  The  spacecraft  is  assumed  uncontrolled  and  will 
move  in  response  to  manipulator  motion.  They  first  assume  a  fixed  inertial  base  at  the 
center  of  mass  of  the  system  and  find  the  Jacobian  of  the  end  effector  written  in  terms  of 
a  coordinate  system  with  its  origin  there  (J*).  When  J*  is  not  of  full  rank,  the  robot  is  in 
a  workspace  location  where  it  is  unable  to  move  its  end-effector  in  an  inertial  direction. 
These  dynamic  singularities  depend  upon  the  inertia  properties  of  the  robot  and  are  also 
path  dependent.  They  are  a  function  of  the  manipulator  joint  space  only  and  do  not 
depend  on  spacecraft  orientation.  The  singularities  consist  of  the  typical  kinematic 
singularities  plus  infinitely  more  dynamically  singular  configurations.  These  are  similar 


24 


to  “inertial  singularities”  discussed  later  in  this  dissertation,  where  the  rigid  robot  cannot 
create  an  interaction  force  or  torque  in  one  or  more  degrees  of  freedom. 

Yoshida,  Nenchev,  and  Uchiyama  [93]  considered  vibration  suppression  control  of  a 
flexible  space  structure  consisting  of  a  robot  mounted  on  a  free-floating  base.  There  are 
two  parts  to  this  work:  reactionless  motion  control  path  planning  and  a  vibration  control 
subtask.  The  first  subtask  involves  a  technique  called  “reaction  null  space”  where  robot 
trajectories  are  selected  to  avoid  creating  dynamic  reaction  forces  at  the  base  of  the 
manipulator.  This  paper  considers  the  interactions  between  the  two  as  a  generic  wrench, 
which  is  a  function  of  the  robot  parameters,  joint  velocities,  and  joint  accelerations.  This 
quantity  is  then  integrated  to  define  the  coupling  momentum  of  the  system.  The  reaction 
null  space  consists  of  trajectories  that  keep  the  coupling  momentum  constant  so  the 
interaction  wrench  is  zero.  In  order  for  these  paths  to  exist,  the  robot  must  have 
kinematic  or  dynamic  redundancy,  a  selective  reaction  null  space  (when  base  flexibility  is 
only  an  issue  in  limited  degrees  of  freedom),  or  a  singular  rigid  inertia  matrix. 
Reactionless  paths  were  determined  for  a  simulated  space  based  robot  and  verified  to  be 
effective  on  their  experimental  testbed,  which  consisted  of  a  two  link  rigid  robot  mounted 
on  a  planar  flexible  base. 

They  also  noted  that  the  orthogonal  complement  of  the  reaction  null  space  could  be 
used  to  achieve  maximum  coupling,  and  thus  could  be  useful  for  the  vibration 
suppression  subtask.  Here  they  assume  the  robot  is  initially  stationary  so  nonlinear 
effects  are  negligible.  Furthermore,  the  flexible  deflections  are  assumed  small  so  the 
inertia  submatrices  are  functions  of  the  rigid  joint  variables  only.  The  vibration  control 
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subtask  commands  the  rigid  inertia  effects  proportional  to  the  base  velocity  so  damping  is 
added  to  the  system.  This  is  similar  to  concepts  by  Lew  and  Moon  [35-38]  and  Sharf 
[63]  that  will  be  discussed  in  section  2.5. 

Yoshida  and  Nenchev  [92]  linked  the  field  of  space  robots  with  the  flexible  base 
manipulator  control  problem.  They  compared  several  types  of  what  they  termed  “under¬ 
actuated  mechanical  systems”,  including  a  flexible  base  manipulator  and  a  free-floating 
manipulator,  and  pointed  out  similarities  and  differences  between  the  two.  The  free- 
floating  robot  was  considered  mounted  to  an  inertia,  while  the  flexible  base  manipulator 
was  considered  a  rigid  robot  mounted  to  a  mass-spring-damper  system.  The  additional 
difference  is  the  existence  of  a  base  constraint  force  for  the  flexible  base  manipulator. 
They  pointed  out  the  “reaction  null  space”  is  a  common  concept  between  the 
configurations.  Thus,  this  concept  could  be  valuable  in  the  case  of  a  redundant 
macro/micromanipulator  to  avoid  or  reduce  disturbances  created  by  commanded 
movements. 


2.5  Micromanipulator  Vibration  Damping  Techniques 
The  complexity  of  the  control  schemes  required  for  macro/micromanipulator  control 
reviewed  in  section  2.3  led  to  an  area  of  research  in  which  the  micromanipulator  is  used 
to  actively  damp  vibration  in  the  macromanipulator.  The  control  scheme  becomes  much 
less  complex  and  the  rigid  robot  actuators  are  typically  able  to  respond  more  quickly  to 
the  vibration.  The  rigid  robot  can  apply  forces  and  torques  directly  to  tip  of  the 
macromanipulator  where  the  vibration  is  usually  largest,  and  this  also  results  in  nearer 


26 


collocation  of  the  actuators  and  the  vibration.  This  technique  can  be  used  to  reduce 
vibration  that  exists  in  the  system  or  is  induced  by  robot  motion  or  external  disturbances. 

2.5.1  Energy  Dissipation  Methods 

Torres,  Dubowsky,  and  Pisoni  [79]  introduced  a  method  entitled  Pseudo-Passive 
Energy  Dissipation  (P-PED)  for  macro/micromanipulator  vibration  control.  They  assume 
locked  macromanipulator  joints  while  the  micromanipulator  performs  its  functions,  so  the 
system  can  be  considered  a  redundant  rigid  manipulator  mounted  on  a  highly  flexible 
supporting  structure.  The  rigid  manipulator  is  first  moved  into  place,  and  then  the 
controller  is  switched  to  the  P-PED  gains.  These  gains  are  chosen  to  maximize  the 
energy  dissipated  by  the  rigid  robot,  essentially  commanding  the  actuators  of  the  robot  to 
behave  as  passive  linear  springs  and  dampers  during  this  phase  of  control.  This  method 
was  shown  effective  in  two  degrees  of  freedom.  However,  this  scheme  is  only  applicable 
to  a  limited  class  of  problems;  i.e.  those  that  allow  the  micromanipulator  to  be  used 
exclusively  for  vibration  damping  when  under  P-PED  control.  After  the  P-PED 
controller  eliminates  the  vibration,  the  original  system  controller  is  used.  In  addition,  this 
technique  uses  measured  rigid  joint  states  only  and  assumes  vibration  in  the 
macromanipulator  is  large  enough  to  create  motion  in  the  micromanipulator.  In  systems 
where  the  actuators  are  highly  nonlinear  or  demonstrate  a  large  amount  of  friction,  such 
as  hydraulic  actuators,  the  base  vibration  will  not  be  observable  in  the  rigid  robot  joint 
motion.  Finally,  it  also  requires  the  full  macro/micromanipulator  model  in  order  to 
determine  the  appropriate  P-PED  gains. 
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Vliet  and  Sharf  [81]  introduced  another  energy  dissipation  method  entitled  impedance 
matching  (IM).  First,  they  developed  an  expression  for  the  power  dissipated  by  the  rigid 

robot,  7*9  ,  where  x  represents  joint  actuation  torques  and  0  represents  the  rigid  robot 
joint  velocities.  By  assuming  constant  amplitude  harmonic  joint  motion  at  a  single 
frequency,  an  expression  for  the  joint  velocities  can  be  found.  Then,  assuming  the  use  of 
a  rigid  joint  PD  controller,  an  expression  for  the  joint  torques  can  be  found  and  PD  gains 
selected  to  maximize  the  power  dissipated  by  the  rigid  robot.  The  same  limitations  apply 
as  for  the  P-PED  method,  except  no  macromanipulator  information  is  needed. 

Vliet  also  discusses  in  his  thesis  limitations  associated  with  the  P-PED  method, 
further  discusses  the  coupling  map  described  in  section  2.4,  and  proposes  some  additional 
measures  of  coupling  [80].  One  is  an  accelerative  damping  measure  based  on  the 
Euclidean  norm  of  the  eigenvalues  of  a  matrix  that  consists  of  the  rigid  robot  inertias  and 
the  macromanipulator  stiffnesses.  Another  coupling  measure  he  proposes  is  a  modal 
inertia  map,  which  is  derived  from  the  joint  torques  required  to  hold  the  rigid  robot  in 
place  as  the  macromanipulator  vibrates.  He  also  presents  in  his  thesis  and  in  [81] 
experimental  work  comparing  the  effectiveness  of  both  the  P-PED  and  IM  methods  in 
damping  vibration  in  a  single  flexible  link  using  a  three  degree  of  freedom  rigid 
manipulator. 

The  P-PED  and  IM  methods  both  assume  the  robot  is  first  moved  into  place  and  then 
the  gains  are  switched  to  the  vibration  control  gains.  Thus,  energy  is  dissipated  from  the 
flexible  manipulator  only  when  the  vibration  control  gains  are  used.  They  also  rely  on 
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the  assumption  that  coupling  effects  between  the  rigid  and  flexible  manipulator  are  large 
enough  to  produce  significant  micromanipulator  joint  motion. 

2.5.2  Inertial  Damping  Control 

These  schemes  use  sensory  feedback  of  the  base  vibration  to  command  the  rigid 
manipulator  to  create  the  appropriate  inertial  interactions  to  actively  control  the  base 
vibration.  Lee  and  Book  [11,33]  developed  a  dual  position  and  vibration  damping 
controller  for  a  macro/micromanipulator  and  proved  its  stability.  They  considered  the 
rigid  robot  from  the  perspective  that  it  has  the  ability  to  apply  “inertial  damping  forces” 
onto  the  tip  of  the  flexible  robot.  Dynamics  were  split  into  slow/fast  submodels.  A  slow 
controller  was  used  to  handle  the  rigid  joint  positions  while  a  fast  controller  was  used  for 
vibration  suppression.  The  rigid  control  gain  matrices  were  carefully  chosen  to  keep  time 
scale  separation  between  the  controller  and  the  flexible  modes  of  vibration.  In  this  case, 
the  rigid  controller  was  critically  damped  and  the  position  controller  chosen  to  be 
approximately  four  times  slower  than  the  fundamental  frequency  of  the  flexible 
manipulator.  The  fast  controller  was  based  on  strain  rate  feedback  of  the  measured 
vibration.  It  was  concluded  that  damping  control  was  best  because  it  is  effective  and 
easier  to  implement  than  a  full  state  feedback  law. 

The  scheme  was  experimentally  verified  for  planar  vibration  on  a  two  link  flexible 
robot,  RALF,  with  a  three  degree  of  freedom  rigid  robot,  SAM  (Small  Articulated 
Manipulator)  mounted  on  its  tip.  Two  links  of  SAM  were  used  to  damp  the  vibration  in  a 
single  fixed  configuration  selected  to  provide  effective  inertial  interaction  forces.  Several 
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items  were  pointed  out  that  still  needed  to  be  addressed  to  extend  the  general  applicability 
of  the  technique.  These  included  limits  on  joint  torques  (actuator  saturation),  required 
joint  travel,  limits  on  actuator  bandwidth,  and  time  scale  separation  between  the  joint 
controller  and  unmodeled  flexible  dynamics. 

Sharf  [63]  recognized  the  interaction  forces  as  the  control  variables  of  interest.  The 
basic  idea  was  that,  given  the  relationship  between  the  rigid  robot  joint  accelerations  and 
the  interaction  forces,  the  appropriate  rigid  body  motion  could  be  commanded  to  modify 
the  dynamics  of  the  flexible  robot  as  desired.  She  showed  in  simulation  the  effectiveness 
of  the  method  by  commanding  the  desired  interaction  forces  to  be: 

Mb\ + Cbx  +  Kbx  =  Fif  =  ~(Gpx  +  Gdx)  (2-4) 

Mb,  Cb,  and  Kb  are  the  macromanipulator  properties,  x  represents  the  flexible  robot 
generalized  coordinates,  and  Fif  are  the  interaction  forces  applied  by  the 
micromanipulator.  Gp  and  Ga  are  the  flexible  motion  feedback  gains.  This  scheme  was 
designed  only  to  damp  the  macromanipulator  vibration  and  would  need  to  be  followed  by 
a  joint  PD  controller  to  dissipate  any  remaining  energy  in  the  system  and  for  rigid  robot 
control. 

Lew  and  Trudnowski  [39]  along  with  Evans  and  Bennett  [40]  added  a  flexible  motion 
compensator  based  on  strain  rate  feedback  of  the  flexible  system  motion  in  parallel  with 
an  existing  rigid  joint  PD  controller.  The  assumption  of  small  motion  of  the  system 
allowed  linearization  about  an  operating  point.  Since  the  micromanipulator  moves 
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relatively  slowly  compared  to  the  fundamental  frequency  of  the  macromanipulator  arm, 
the  flexible  dynamics  were  assumed  negligible  during  commanded  joint  motion.  The 
joint  control  loop  was  first  closed  and  the  flexible  controller  designed  from  the  closed 
loop  transfer  function  of  the  system.  It  was  shown  that,  as  long  as  the  flexible  motion 
controller  is  designed  to  be  stable,  the  joint  controller  would  also  be  stable.  The  vibration 
compensator  was  designed  to  add  damping  to  the  first  mode  while  limiting  the  bandwidth 
to  avoid  exciting  higher  modes.  The  resulting  flexible  motion  compensator  takes  the 
form: 


Cf(s)  = 
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where  Ti  and  T2  are  time  constants  used  to  remove  steady-state  offsets  and  decrease  high 
frequency  gain,  respectively.  Additional  lead-lag  blocks  were  also  needed  for  proper 
phase  compensation.  This  signal  was  then  added  to  the  joint  PD  controller. 

Raab  and  Trudnowski  [58]  considered  an  active  damping  technique  using  inertial 
torques  generated  by  torque  wheels  mounted  at  the  end  of  a  single  flexible  link.  They 
studied  the  flexible  mode  suppression  only.  They  were  able  to  demonstrate  two  degrees 
of  freedom  of  vibration  damping  under  varying  payload  masses.  The  vibration  was 
sensed  using  strain  gage  pairs  near  the  hub  of  the  link.  The  resulting  flexible  motion 
compensator  took  the  form: 

Ks 
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where  p  was  chosen  to  provide  optimum  phase  compensation  at  the  mid-loading  point. 
Both  of  these  techniques  showed  promising  results  for  vibration  control  in  two  degrees  of 
freedom  under  certain  conditions. 

Cannon  [15]  furthered  the  concept  of  inertial  damping  to  include  an  inverse  dynamics 
model,  helping  reduce  the  variation  in  performance  throughout  micromanipulator’s 
workspace.  He  developed  and  demonstrated  the  effectiveness  of  the  combined  position 
and  vibration  controller  in  one  degree  of  freedom  on  a  flexible  link  with  a  single  link 
rigid  robot  mounted  to  its  tip  to  provide  the  vibration  damping.  The  rigid  position  control 
scheme  was  chosen  with  stiff  PD  gains  so  the  closed  loop  natural  frequency  of  the  system 
was  approximately  ten  times  the  frequency  of  vibration  to  be  controlled.  Acceleration 
measured  at  the  tip  of  the  flexible  link  was  used  for  feedback  of  the  base  vibration.  In 
this  case,  the  resulting  vibration  controller  took  the  form: 


Tf  = 


Kx 
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where  x  represents  the  measured  base  vibration  in  a  single  degree  of  freedom,  0  is  the 
position  of  the  single  flexible  link  rigid  robot,  and  the  denominator  represents  the  rigid 
robot  dynamics.  This  control  torque  is  then  added  to  the  total  PD  joint  control  torque. 

Cannon  noted  three  disadvantages  of  using  this  method  alone:  it  does  not  reduce  the 
maximum  amplitude  of  the  vibration  or  the  control  effort  needed,  and  can  increase  the 
settling  time  of  the  joint  angle  response.  He  also  noted  decreased  improvement  in 
damping  as  the  joint  PD  gains  are  increased.  He  also  combined  the  inertial  damping 
method  with  command  shaping  techniques  to  show  that  the  combination  could  provide 


32 


both  vibration  damping  and  amplitude  reduction.  The  conclusion  was  that  the  use  of  the 
inertial  damping  technique  does  not  preclude  the  use  of  another  control  technique  if  an 
additional  performance  measure,  such  as  vibration  amplitude,  is  a  concern.  These 
techniques  were  also  later  applied  to  a  macro/micromanipulator  at  Pacific  Northwest 
Laboratory  [16]  and  showed  similar  improvements  in  performance. 

Loper  and  Book  [12,42]  extended  the  inertial  damping  scheme  to  two  degrees  of 
freedom  of  vibration.  They  used  the  same  control  technique  as  Cannon  except 
accelerations  were  measured  in  two  directions  and  two  links  of  a  three  degree  of  freedom 
robot  were  used.  The  controller  took  the  form  of  equation  2-7,  except  accelerations  in 
two  directions  were  used  and  the  rigid  robot  dynamics  were  modeled  using  two  links  of 
the  robot.  This  technique  was  shown  experimentally  to  be  effective  for  planar  vibration, 
again  under  certain  conditions. 

Lew  and  Moon  [35-38]  have  recently  considered  the  more  general  case  of  a  robot 
attached  to  a  passive  compliant  base,  but  only  allow  three  degrees  of  freedom  of  base 
translation.  The  scheme  compensates  for  base  vibration  while  following  a  desired 
position.  Real-time  estimates  of  the  nonlinear  rigid  body  dynamics  are  computed  from 
joint  accelerations  calculated  from  measured  optical  encoder  position  data.  The  coupled 
rigid  body  equation  of  motion  (last  row  of  equation  2-3)  can  be  written  as: 

BT(6)6  +  N(0, 6, q,q)  =  T  (2-8) 
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Again,  instead  of  the  notation  used  in  his  paper,  notation  used  here  is  consistent  with  the 
notation  introduced  in  equations  2-1  and  2-2.  N  represents  the  rigid  robot  nonlinear, 
gravitational,  and  coupling  terms.  The  new  rigid  robot  command  torque  becomes: 

t  =  t  p-BT(0)6+u  (2-9) 

where  tp  represents  the  previously  commanded  joint  torques,  Bz  is  the  estimated  rigid 
robot  inertia  matrix,  and  u  is  the  new  rigid  robot  control  input.  Using  Equation  2-9  in  2-8 
and  ip  to  estimate  the  nonlinear  terms,  the  new  equations  of  motion  (equation  2-3 
rearranged)  become: 
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A  two-time  scale  controller  is  then  applied  to  the  partially  decoupled  models  with  a  fast 
controller  for  rigid  link  position  and  a  slow  controller  for  the  vibration  controller.  The 
new  rigid  robot  control  input,  u,  is  commanded  so  the  joint  accelerations  are  proportional 
to  the  flexible  base  velocity  and  damping  is  added  to  the  system.  Note  to  find  the 
required  joint  control  input,  u,  according  to  equation  2-9,  requires  the  joint  accelerations, 
which  may  be  noisy  and  difficult  to  acquire  real-time  if  only  joint  position  measurements 
are  available. 

Of  course,  another  area  that  is  only  briefly  mentioned  here  is  hardware  modifications 
such  as  smart  structures  or  passive  damping,  which  may  be  the  only  solution  to  certain 
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problems.  One  example  may  be  if  the  controlled  motion  is  fully  prescribed  and  no 
deviation  is  allowed  for  path  modifications  or  to  damp  the  vibration.  Another  example 
would  be  if  the  vibration  is  not  controllable  at  the  point  of  interface  with  the  rigid  robot 
or  if  the  vibration  is  not  observable  (the  micromanipulator  or  sensors  can  only  be  placed 
at  a  node  point).  Other  cases  could  occur  when  the  vibration  controller  is  designed  based 
on  a  reduced  order  model  and  fails  to  compensate  for  excitation  of  higher  modes  in  the 
system.  In  these  cases,  another  option  may  be  to  introduce  a  dissipation  mechanism  to 
enhance  damping  rather  than  modify  the  control  loop  [9,10]. 

2.6  Limitations  of  Previous  Research 

Control  of  macro/micromanipulators  has  been  investigated  using  many  different 
techniques.  As  noted  previously,  inertial  damping  schemes  using  the  micromanipulator 
to  damp  the  vibration  is  an  attractive  compromise  between  control  system  complexity  and 
system  performance.  These  schemes  have  been  developed  and  demonstrated  for  a  very 
specialized  class  of  systems  in  unique  configurations.  Some  of  the  limitations  of  the 
research  performed  so  far  in  this  area  are: 

1.  Full  macromanipulator  flexibility  (translational  and  rotational)  has  not  been 
considered.  Lew  and  Moon  [35-38]  have  taken  the  more  general  approach  of  considering 
the  macromanipulator  as  a  compliant  base,  but  limit  the  motion  to  only  translational 
motion,  which  greatly  simplifies  the  problem.  The  space  robotics  work  by  Yoshida  et  al 
[92,93]  considers  the  case  of  a  robot  mounted  on  a  floating  base  and  has  lent  some 
valuable  theoretical  work  to  this  area,  but  detailed  work  is  yet  to  be  seen. 
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2.  The  variation  in  performance  throughout  the  workspace  has  not  been  fully 
addressed.  In  particular,  there  are  locations  in  the  workspace  where  the  ability  of  the 
manipulator  to  generate  effective  interaction  forces  and  torques  is  diminished.  There  has 
been  some  work  in  space  robotics  by  Torres  et  al  [77,78]  and  Vliet  [80]  that  begins  to 
address  this  issue.  A  rigorous  investigation  of  these  singularities,  similar  to  the  work 
done  by  Papadopoulos  and  Dubowsky  [56]  for  space  dynamic  singularities,  is  lacking.  In 
addition,  there  has  not  been  a  method  proposed  to  address  how  to  use  the  variation  in 
performance  to  improve  the  effectiveness  of  the  damping  scheme. 

3.  Most  of  the  above  schemes  have  assumed  ideal  system  modeling  for  development 
of  the  vibration  control  scheme.  Lew  and  Moon  [35-38]  looked  at  estimating  nonlinear 
and  coupling  effects  but  practical  implementation  of  this  method  could  become 
challenging.  There  still  needs  to  be  a  detailed  investigation  of  the  coupled  system  model 
as  well  as  an  evaluation  of  the  robustness  of  the  control  scheme  to  modeling  errors. 

4.  Few  hardware  demonstrations  have  been  performed  on  realistic 
macro/micromanipulators.  Those  that  have  been  were  on  robots  in  specific 
configurations  and  with  only  one  or  two  vibrational  degrees  of  freedom.  In  addition,  the 
only  two  degree  of  freedom  demonstrations  have  been  on  robots  that  have  naturally 
decoupled  inertial  damping  performance.  The  effects  of  (1-3)  may  become  even  more 
important  with  additional  degrees  of  freedom. 

5.  Methods  to  maximize  the  amount  of  damping  provided  by  the  vibration  controller 
have  not  been  addressed.  In  addition,  practical  limitations  such  as  actuator  saturation  and 
limits  on  joint  travel  have  not  been  addressed.  In  fact,  very  little  guidance  is  available  on 
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choosing  the  vibration  control  gains  or  establishing  limits  on  gains  that  consider  these 
factors. 

6.  The  impact  of  the  vibration  damping  controller  on  the  position  controller  has  not 
been  systematically  analyzed.  Some  researchers  have  chosen  to  consider  the  vibration 
problem  separately  [12,15,63,81,93]  while  others  have  carefully  chosen  the  position 
control  and  vibration  control  loops  to  have  time  scale  separation  [1 1,33,35]. 

Of  course,  there  is  plenty  of  room  for  additional  work  in  this  area.  This  dissertation 
seeks  to  address  some  of  the  major  issues  remaining  in  the  field.  In  particular,  this 
research  includes: 

1.  Consideration  of  the  multi-degree  of  freedom  (DOF)  macro/micromanipulator 
control  problem  by  studying  the  analogous  problem  of  a  rigid  robot  mounted  on  a  flexible 
base. 

2.  Investigation  of  the  variation  in  performance  throughout  the  workspace  and 
inertial  singularities,  or  locations  in  the  workspace  where  the  rigid  robot  loses  its  ability 
to  create  effective  interactions  in  one  or  more  degrees  of  freedom. 

3.  Development  of  a  control  scheme  that  provides  active  base  vibration  damping  in 
parallel  with  position  control  and  establishment  of  appropriate  vibration  control  gain 
limits. 

4.  Simulation  and  experimental  work  on  a  realistic  multi-degree  of  freedom 
macro/micromanipulator. 
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CHAPTER  III 


FLEXIBLE  BASE  MANIPULATOR 
MODELING 

3.1  Introduction 

This  chapter  describes  modeling  of  a  rigid  robot  mounted  on  a  flexible  base.  The 
flexible  base  represents  a  multi-link  macromanipulator  with  locked  joints.  The  rigid 
robot  will  provide  the  fine-tuned  end  point  position  control.  In  this  research,  it  is  also 
used  to  damp  vibration  in  the  flexible  base.  Thus,  it  is  especially  important  to 
characterize  the  dynamic  interactions  between  the  macro  and  micromanipulator  and  in 
order  to  do  this  the  system  needs  to  be  modeled. 

First,  a  recursive  Lagrangian  approach  is  described  which  is  used  to  model  the 
macromanipulator.  This  approach  is  commonly  used  in  flexible  robot  modeling  and 
hence  is  only  briefly  reviewed  here.  Next,  the  micromanipulator  model  is  developed 
using  a  recursive  Newton-Euler  algorithm.  The  important  dynamic  effects  that  need  to  be 
characterized  are  the  required  joint  torques  to  operate  the  robot  and  the  interaction  forces 
and  torques  acting  at  the  base  of  the  robot.  The  joint  torque  equations  are  the  same  form 
as  those  for  a  fixed  base  rigid  robot  with  additional  coupling  terms  due  to  the  base 
motion.  The  interaction  forces  and  torques  are  explicitly  solved  for  and  are  the  subject  of 
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further  investigation  in  Chapter  4.  Chapter  5  then  investigates  how  to  control  these 
interactions  to  damp  the  base  vibration. 

3.2  Flexible  Base  Model 

The  flexible  base  represents  a  multi-link  flexible  manipulator  with  locked  joints.  The 
method  chosen  here  is  a  recursive  Lagrangian  formulation  using  a  finite  number  of 
assumed  modes,  which  is  applicable  to  an  n-DOF  flexible  link  manipulator.  The 
advantage  of  this  method  is  its  ability  to  include  flexible  link  deformation.  This  method 
is  well  documented  and  more  detailed  descriptions  may  be  found  in  the  paper  by  Book 
[8]  and  others  who  have  used  this  method  to  model  flexible  manipulators  [15,32,34,42, 
52,53,62,72].  However,  other  methods  may  be  used  provided  they  yield  inertia  and 
stiffness  properties  and  adequately  capture  the  significant  dynamics  of  the 
macromanipulator.  Modal  damping  estimates  are  also  often  added  and  could  be  based  on 
experimental  results  or  estimated  from  material  properties. 

The  key  difference  between  a  flexible  and  rigid  robot  is  its  continuous  nature.  <  Both 
rigid  joint  motion  and  elastic  deflections  govern  the  motion  of  a  flexible  robot,  so  it 
theoretically  has  an  infinite  number  of  degrees  of  freedom.  However,  it  is  necessary  to 
develop  a  more  manageable  model  that  approximates  the  system,  yet  adequately  captures 
the  significant  dynamics.  The  approach  described  here  begins  with  assuming  an 
appropriate  number  of  modes  to  model  the  flexibility  in  each  link  for  each  degree  of 
freedom.  The  position  of  an  arbitrary  point  on  each  link  is  composed  of  summations  of 
the  assumed  mode  shapes  multiplied  by  the  generalized  coordinates.  These  are  used  to 
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form  the  kinetic  and  potential  energy  of  the  system  and  with  Lagrange’s  equations  yield 
the  equations  of  motion  of  the  system.  It  is  assumed  for  this  work  the  resulting 
macromanipulator  model  can  be  linearized  about  an  operating  point,  i.e.  at  a  fixed  joint 
configuration  and  neglecting  the  flexible  generalized  coordinates  in  the  inertia,  stiffness, 
and  damping  matrices.  It  is  further  assumed  the  flex  model  is  developed  referenced  to  an 
inertial  coordinate  frame  coincident  with  its  last  link,  or  at  the  base  of  the 
micromanipulator.  The  resulting  matrices,  although  constant  at  a  given  locked  joint 
configuration,  will  generally  be  fully  coupled  and  the  generalized  coordinates  will  only 
include  the  flexible  states. 

It  is  assumed  each  link  could  have  two  degrees  of  freedom  of  transverse  vibration 
plus  torsion  about  the  z-axis  (Figure  3-1)  and  that  axial  vibration  is  negligible.  Thus, 

n  3 

there  will  be  a  total  of  X  X  >*  equations  of  motion  and  generalized  coordinates, 

/  =  1  k  =  1 

where  n  is  the  number  of  macromanipulator  links,  k  represents  vibration  in  the  x,  y,  or  0Z 
direction,  and  mjk  is  the  number  of  assumed  modes  for  the  kth  direction  of  vibration  of  the 
ith  link. 

3.2.1  Assumed  Modes 

The  first  task  is  to  assume  an  appropriate  set  of  modes  for  each  degree  of  freedom. 
The  mode  shapes  could  be  based  on  many  methods,  such  as  Ritz  series,  finite  element 
models,  or  analytical  results  for  continuous  systems  [24].  The  chosen  set  of  modes  only 
needs  to  be  linearly  independent  and  satisfy  the  system  geometric  boundary  conditions 
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Figure  3-1 

Flexible  Link  Model  Notation 


[13].  A  reasonable  method  of  estimating  assumed  modes  for  flexible  manipulators  is 
based  on  Euler-Bemoulli,  or  classical  beam  theory.  In  order  for  this  to  apply,  the 
centroids  of  each  link  must  lie  along  the  z-axis  in  the  undeformed  state.  In  addition,  the 
cross-sectional  dimensions  must  be  small  relative  to  the  length  of  the  link  and  the  cross 
sectional  shape  should  vary  little  along  the  z  direction  [24].  These  are  often  reasonable 
assumptions  for  a  macromanipulator,  which  is  characterized  by  its  long,  lightweight 
links.  Transverse  and  torsional  vibration  mode  shapes  for  flexible  beams  subject  to 
common  boundary  conditions  may  be  found  in  many  references,  including  Ginsberg  [24], 
Meirovitch  [49],  Rao  [59],  and  Weaver,  Timoshenko,  and  Young  [83]. 
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The  governing  differential  equation  of  motion  for  transverse  vibration  of  a  uniform 


beam  [83]  is: 


£/"‘" pA^A~  o 


dz 


dz 4 


(3-1) 


u(z,t)  describes  the  resulting  vibration  in  the  x  direction  (or  v(z,t)  in  the  y  direction),  as 
defined  in  Figure  3-1,  for  the  ith  link.  E  is  modulus  of  elasticity,  p  is  mass  density,  A  is 
the  cross-sectional  area  and  I  is  the  moment  of  inertia  of  the  link  about  the  x  or  y  axis  (all 
assumed  constant).  Using  separation  of  variables, 


(3-2) 


a  basis  set  of  mode  shape  functions  can  be  found  of  the  form: 

<f>j  =  CJX  sin(/?z)  +  CJ2  cos(/?yz)  +  Cj3  sinh(/?;z)  +  CJ4  cosh (fyz) 


(3-3) 


CjS  and  PjS  are  determined  from  the  eigenvalue  problem  appropriate  for  the  system’s 
boundary  conditions,  m*  should  be  truncated  to  a  reasonable  number  of  modes  to 
adequately  model  the  flexible  dynamics  without  unnecessarily  increasing  model  size  and 
complexity.  Some  researchers  have  found  that  two  or  three  modes  suffice  to  represent 
flexible  dynamics  on  relatively  uncomplicated  systems  [13],  based  on  the  low  amplitude 
of  the  higher  frequency  modes. 

Common  boundary  conditions  used  for  modeling  flexible  manipulators  are  fixed/ffee 
or  pinned-pinned,  but  other  boundary  conditions  may  be  more  applicable  depending  on 
the  specific  application.  For  example,  fixed/ffee  boundary  conditions  are: 
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<j>j  (0)  =  0  (no  motion  at  attach  point) 

d+A 0) 

—  — -  =  0  (zero  velocity  at  attach  point) 


dz 


d2*j(0) 


dz 2 


o 

dz 3 


=  0  (no  bending  moment  at  attach  point) 
(no  shear  force  at  attach  point) 


(3-4) 


Using  these  boundary  conditions  in  equation  3-3  results  in  a  set  of  4x4  equations  that 
yield  the  following  eigenfunctions,  or  assumed  modes: 


<t>j  =  Cj  jsinO^z)  -  sinh(/?yz) + 
Cj  are  chosen  such  that  tf>j  =  1 


cos  (fijL) + cosh (J3jL) 
sin  ifijL) + sinh (J3jL) 
atz=L  (link  length) 


(3-5) 


The  first  four  natural  frequencies  for  transverse  vibration  of  a  beam  with  fixed-free 
boundary  conditions  are  [83]: 


#1  =  1.875104 
PjL  =  4.694091 
P,L  =  7.854757 

P,L  =  10.995541  (3-6) 

The  other  issue  that  could  be  important  for  flexible  manipulators  is  torsional  beam 
vibration.  Assuming  a  uniform  shaft  with  uniform  cross  section,  the  free  vibration  is 
governed  by  [49]: 


G  dj^z, t)  _  d2e,(z,t) 

p  dz2  dt 2  1  ’ 

where  G  is  the  shear  modulus  of  elasticity  (assumed  constant)  and  0Z  describes  the 
rotation  of  the  flexible  link  about  the  z-axis.  Again,  separation  of  variables  allows 
solving  for  the  mode  shapes,  which  take  the  form: 
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(3-8) 


^  CO  ,Z  CO  ,z 

Xj(z)  =  A j  cos  -  I-  B j  sin — — 


The  frequencies  again  depend  on  the  boundary  conditions  and  for  fixed/ffee  boundary 
conditions,  the  mode  shapes  take  the  form: 

=  z)  (3-9) 

The  motion  of  the  ith  link  can  now  be  described  as  a  summation  of  the  assumed 
modes  and  associated  generalized  coordinates. 


f»ik 

«/(*/>  o = 

i= 1 

;=! 


^(2,»0=Z^(z/K(0  (3-10) 

7=1 

Uj  and  Vj  are  the  resulting  ith  link  deflection  in  the  x  and  y  directions  and  9zj  the  torsional 
rotation  about  the  z-axis.  pjj,  qy,  and  sy  are  the  time-varying  amplitudes  of  mode  j  of  link 
i  in  the  x,  y,  and  0Z  directions. 


3.2.2  Kinetic  and  Potential  Energy 

Next  Lagrange’s  equations  will  be  used  to  extract  the  inertia  and  stiffness  properties 
of  the  macromanipulator.  First,  kinetic  and  potential  energy  expressions  need  to  be 
formed  for  each  flexible  link.  The  kinetic  energy  terms  represent  distributed  translational 
and  rotational  energy,  while  the  potential  energy  terms  represent  elastic  bending  effects 
and  gravity. 
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First,  an  arbitrary  position  on  the  macromanipulator  needs  to  be  described  relative  to 
the  fixed  inertial  coordinate  system  xn,  y.i,  z\.\  in  Figure  3-1.  The  coordinate 
transformation  between  links  involves  both  the  rigid  joint  transformations  (Aj)  as  well  as 
link  deflection  transformations  (Ej).  Book  [8]  described  a  practical  means  of  doing  this 
by  using  4x4  homogeneous  transformation  matrices  to  describe  the  position  and 
orientation  of  one  coordinate  frame  with  respect  to  another. 

Using  the  notation  introduced  in  his  paper  and  referenced  to  the  fixed  inertial 
coordinates  at  the  base  of  the  macromanipulator,  the  position  of  a  point  on  link  i  can  be 
written  as: 


h  =  W,ti 


hf  =  point  referenced  to  fixed  frame  = 


h'  =  point  referenced  to  link  i  frame  = 


(3-11) 


Uj  and  Vj  are  as  defined  in  equation  3-10  and  z\  is  the  distance  along  link  i.  The 
transformation  Wj  can  be  split  into  rigid  and  flexible  components,  as  defined  in  Figure  3- 
1,  or: 

(3-12) 

For  this  research,  A;  is  assumed  fixed  at  a  specific  macromanipulator  joint  configuration. 
Ej,  however,  must  include  both  link  deflections  as  well  as  rotations.  The  rotation  that 
occurs  between  i  and  i-1  is  due  to  the  small  rotations  at  the  tip  caused  by  the  transverse 
vibration  as  well  as  the  torsional  vibration. 
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Assuming  small  rotation  angles,  the  direction  cosine  matrix  simplifies  and  the 


complete  link  transformation  matrix  can  be  written  as: 


T 
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where  L,  is  the  length  of  link  i.  The  assumption  of  small  rotations  gives: 


0,/(V)  *  Z 


7=1 


dz. 


AdS..(z.) 

Oyi(Zi>t)*  Z  l  ~  PiA 0 


(3-13) 


(3-14) 


However,  if  the  macromanipulator  has  multiple  links,  it  will  be  more  useful  to  write 
the  position  vector  referenced  to  a  fixed  coordinate  system  coincident  with  the  last  link  of 
the  macromanipulator  (xn,  yn,  zn)  in  its  equilibrium  position  (E=I).  Thus,  the  position 
vector  can  be  found  from: 


(3-15) 


0  is  found  the  same  way  except  using  the  rotational  terms  only  in  Ej  (3x3s).  The  potential 
and  kinetic  energy  of  the  system  can  now  be  written  as: 
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where: 


p  =  mass  density 
A  =  cross-sectional  area 
E  =  modulus  of  elasticity 

Ix,Iy  =  area  moment  of  inertia  of  the  cross  sectional  area  computed  about  the  x  or  y  axis 
G  =  modulus  of  rigidity 

J  =  polar  area  moment  of  the  cross  sectional  area  computed  about  the  neutral  axis 

One  of  the  benefits  of  this  method  is  that  as  much  detail  can  be  included  in  the  equations 
of  motion  as  desired.  Here  many  of  the  assumptions  that  have  been  used  and  justified  by 
other  researchers  have  been  used  to  pare  the  equations  of  motion  down  to  the  most 
significant  contributions  [8,24,34,46,52,72,83]. 

Now  Lagrange's  equations  can  be  used  to  derive  the  equations  of  motion. 


d_(dT\_dT_  dV_ 
dt  \  5qi )  dqt  +  dqi 


(3-17) 


Qi  are  the  nonconservative  generalized  forces  applied  to  the  macromanipulator 
corresponding  to  the  generalized  coordinate  qj.  In  this  research,  these  are  the  interaction 
forces  and  torques  created  by  the  micromanipulator.  The  generalized  forces  are 
determined  from  the  virtual  work  done  by  the  micromanipulator,  or 

SWork  -  ¥IPSr + tifS@  (3-18) 


The  infinitesimal  displacements  and  rotations  at  the  tip  of  the  macromanipulator  are 
given  by  equation  3-15  evaluated  at  Zi=Li.  The  partial  derivatives  of  the  resulting 
infinitesimal  displacements  and  rotations  are  taken  with  respect  to  each  generalized 
coordinate  to  find  the  generalized  forces. 
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The  final  equations  of  motion  take  the  form: 

M(q)q  +  C(q)q  +  £(q)q  =  Q  (3-19) 

where  it  is  assumed  the  mass,  stiffness,  and  damping  matrices  can  be  linearized  about  a 
nominal  operating  point.  Since  the  joints  of  the  macromanipulator  are  locked,  q 
represents  the  flexible  states  and  consists  of  a  finite  number  of  modes  of  interest.  The 
mass,  stiffness,  and  damping  matrices  can  be  linearized  and  assumed  approximately 
constant  about  an  operating  point,  q. 

3.3  Flexible  Base  Rigid  Manipulator  Model 
In  order  to  better  understand  and  analyze  the  coupling  effects  between  a  rigid 
manipulator  and  flexible  macromanipulator,  the  problem  was  generalized  into  a  rigid 
manipulator  mounted  on  a  flexible  base  (Figure  3-2).  The  base  represents  inertia, 
stiffness,  and  damping  properties  of  a  distributed  parameter  system,  modeled  as  discussed 
in  section  3.2.  There  are  two  goals  of  this  portion  of  the  work.  The  first  goal  is  to 
develop  the  equations  of  motion  in  order  to  model  and  simulate  the  interactions  between 
the  robot  and  its  flexible  base.  The  second  is  to  investigate  these  effects  in  order  to 
determine  which  are  most  significant.  The  analysis  of  the  interaction  forces  and  torques 
is  discussed  in  Chapter  4. 

A  recursive  Newton-Euler  method,  commonly  used  to  develop  joint  torque  equations 
for  rigid  robots  [18,48,61],  was  used  to  find  the  interaction  forces  and  torques.  Other 
methods  are  also  valid  if  they  allow  solving  for  the  interaction  forces  and  torques  that  act 
at  the  base  of  the  micromanipulator.  It  is  assumed  that  the  origin  of  an  inertial  coordinate 
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system  is  located  at  the  base  of  the  rigid  robot,  or  at  the  tip  of  the  undeformed 
macromanipulator.  The  elastic  states  of  the  macromanipulator  affect  the  rigid  robot  by 
moving  its  base  in  Cartesian  space  (Figure  3-2).  In  developing  the  equations  of  motion, 
these  become  boundary  conditions  on  the  rotational  velocities  and  translational  and 
rotational  accelerations  of  the  first  link  and  are  then  propagated  forward  to  the  other 
links.  Then,  a  backward  recursion  is  used  to  solve  for  the  forces  and  torques  acting  on 
each  link,  with  the  final  set  giving  the  forces  and  torques  acting  between  the  rigid  robot 
and  the  macromanipulator. 

The  assumptions  made  in  the  development  of  these  ideal  equations  of  motion  are: 

1)  No  contact  forces  or  moments  are  applied  to  the  tip  of  the  micromanipulator 

2)  Off-diagonal  products  of  inertia  are  negligible 

3)  Position  vectors  for  off-axis  distances  to  link  centers  of  gravity  (CGs)  are  negligible 
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4)  Friction  effects  and  other  nonlinearities  are  not  modeled 

5)  Rotor  inertia  effects  are  not  included. 

If  actuator  dynamics  dominate  the  rigid  robot  performance,  an  alternate  form  of  the  rigid 
robot  model  may  apply.  However,  the  interaction  forces  and  torques  described  by  this 
model  will  still  be  valid. 

First,  consider  for  the  moment  a  single  link  rigid  robot  mounted  to  a  flexible  base. 
The  base  is  free  to  move  in  any  direction.  The  arm  rotates  about  the  x-axis  as  defined  in 
Figure  3-3  (out  of  the  page). 


Figure  3-3 

Flexible  Base  Single  Link  Rigid  Robot 

The  acceleration  of  the  center  of  gravity  of  the  link  is  given  by: 

ac  =  a0  +  a  x  rcc  +  a  x  (a  x  rco) 
where: 

a0  =  xi  +  yj  +  (z  +  g)k 
(0  =  (6X  +  9)i  +  0yj  +  ezic 

a  =  (0X  +0)i  +  eyj  +  ej  (3-20) 
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where: 


aG  =  acceleration  of  the  center  of  gravity  of  the  rigid  link 
ao  =  acceleration  of  the  base 
g  =  gravitational  constant 
co  =  angular  velocity  of  the  link 
a  =  angular  acceleration  of  the  link 

rcG  =  position  vector  from  the  base  to  the  center  of  gravity  of  the  link 


Now  the  interaction  forces  and  torques  at  the  base  of  the  link  can  be  solved  for: 


^  if  ~  ™  ®  a 


t  ,r  =  -r 


CG  *  ^  IF  + 


Ijy 

Ij\ 


(3-21) 


where  m  is  the  mass  of  the  robot  arm  and  Ixx,  Iyy,  and  Izz  are  moments  of  inertia  of  the  link 
about  the  respective  axes.  Just  for  this  one  link  case,  the  interaction  forces  take  the  form: 


1 

0 

o' 

x 

'  0  " 

'  0 

-red 

-rsd 

"  0 

0 

0 

0 

- ! 

*  N> 

0 

1 

0 

y 

+ 

red 

0  + 

rc6 

0 

0 

K 

+ 

-rsd 

d2  + 

-rsd 

0 

-rsd 

d2 

y 

0 

0 

1 

Z 

rsd 

rs8 

0 

0 

A. 

rcO 

red 

red 

0 

« 

"rsd 

-red 

0  " 

i 

i _ 

0 

rsd 

-red' 

~¥ 

'o' 

0 

0 

-red 

dd 

X  z 

+ 

-2rsd 

0 

0 

eye 

+ 

0 

0 

0 

rsd 

1 

1 _ _ 

2rcd 

0 

0 

i - 

Cb. 

\ 

_g. 

Here  the  abbreviated  form  for  the  trigonometric  functions,  c0  and  s0,  are  used  to 
represent  cos0  and  sin0,  as  will  be  the  case  for  the  remainder  of  this  document.  Using  the 
notation  for  the  macromanipulator  in  the  previous  section  and  equations  3-10  and  3-14, 
the  displacements  and  rotations  at  the  tip  of  the  macromanipulator  can  be  written  in  terms 
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of  the  generalized  coordinates,  q.  Thus,  these  equations  can  be  written  in  more  compact 
form  as: 


Fif  Bf{6)  m+rNfV)-\.2[Nfcl(8)  1  \Nfc2(e,0)  1  [G/WI 

rIF\  jCrt«?)  SrO(0)JL<9j  L^oWj  [Kocm  [Ntoc2(d,0)\  L^o(^)J 


The  goal  of  the  control  scheme  is  to  control  the  interaction  forces  and  torques  via  the 
rigid  robot  to  damp  vibration  in  the  macromanipulator.  From  the  above  reduced 
equations  of  motion,  it  is  apparent  the  effects  that  directly  influence  the  interaction  forces 
and  torques  are  given  by: 


Bf(0)  Nf(6) 

0  + 

BJ0)\  L^roWj 


e 2 


(3-24) 


The  generic  algorithm  is  similar  to  the  well-documented  Newton-Euler  method  for 
deriving  the  equations  of  motion  for  a  rigid  robot,  except  in  this  case  the  interaction 
forces  and  torques  are  required  in  addition  to  the  joint  torque  equations.  The  notation 
used  below  is  consistent  with  that  found  in  Sciavicco  and  Siciliano  [61]  where  the  more 
efficient  method  of  referring  the  vectors  to  the  current  frame  associated  with  link  i  is 
used.  The  algorithm  uses  forward  recursions,  which  propagate  the  velocities  and 
accelerations  of  each  link  forward  to  the  next  link.  This  is  followed  by  backward 
recursions,  which  solve  for  the  forces  and  torques  acting  on  each  link  starting  with  the 
force  and  moment  applied  to  the  end  effector  (assumed  zero  for  this  work). 
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Using  parameters  as  defined  in  Figure  3-4,  the  forward  recursions  take  the  form: 


,•  +0,zo]  revolute 

0)t 

[(R,- _1  )T  prismatic 

. .  [(R^Yltyzl+diZQ+Op1^  x  z0]  revolute 

cbi  =  < 

[(R,-1)7^!"1  prismatic 

...  [(/?/"' )Tpl:]+d)‘  x  r*_x  +  co\  X  (©'  X  r/_,)  revolute 


(R/”1)r(jJ!_,i  +  d(z0) +  2^0)1  x  (R!  1)rz0  +  &>!  x  r/_,  +  x  (&!  x  r/_j)  prismatic 


Pii=Pl+<*>l  x  ^-+^-  x  (©/  x  rc\) 


(3-25) 


ri-l 


where: 


z0  =  unit  vector  of  joint  0  axis  =| 


^  =  angular  velocity  of  link  i 

co\  -  angular  acceleration  of  link  i 

p\  -  linear  acceleration  of  the  origin  of  frame  i 

plci  =  linear  acceleration  of  the  center  of  mass  of  link  i 
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The  backward  recursions  take  the  form: 


fi  =  KJm  +  m,P'ci 

*lFi=RL*IFM+RLfi+ 1  X  rc\-fi  X  rc\  +  Ifi)\  +  (o\  X 

T  \rm(Rl~')T  z0  revolute 

{fliK'Y  zo  prismatic 

fi  =  interaction  force  between  links  i  and  i-1 
rrFi  =  interaction  torque  between  links  i  and  i-1 

Tj  =  generalized  joint  actuation  torque  for  link  i  (3-26) 

Boundary  conditions  of  the  tip  velocities  and  accelerations  of  the  macromanipulator  are 
applied  to  the  first  link  and  propagated  through  the  forward  recursions.  It  is  assumed  that 
the  micromanipulator  is  not  in  contact  with  any  object  so  the  forces  and  torques  applied 
to  the  tip  of  its  last  link  are  zero. 

The  equations  of  motion  take  the  general  form: 

FIF=Bf(6j6  +  Nf(6,biej)  +  Cfm  +Nfc(q,q,d,d) 
f  if  =BJ6jd  +  NJ6,di0j)  +  Cxam  +  NTOc(q,q,6,6) 

T  =  BT(d)e  +  Nt(d,6fij)  +  CT(d)q  +Ntc( q,q,6,6)  (3-27) 

0  represents  the  rigid  robot  joint  variables  and  q  represents  the  flexible  base  generalized 
coordinates.  Bf,  BTo,  Cf,  and  CTo  represent  inertia  effects  of  the  micro  and  macro 
manipulators,  respectively.  Bf  and  Bxo  are  particularly  important  because  they  represent 
the  controllable  rigid  robot  inertia  effects.  These  matrices  are,  in  general,  not  symmetric, 
or  positive  definite  (but  the  inertia  matrix  for  the  complete  coupled  system  is,  of  course). 
The  remaining  terms  in  3-27  represent  nonlinear  and  gravitational  effects.  The  third 
equation  is  the  typical  joint  torque  equation  with  extra  coupling  terms.  Often  actuator 
dynamics  or  other  effects  dominate  the  robot  performance,  so  this  equation  could  take 
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other  forms.  However,  for  this  work  it  is  assumed  the  relationship  between  the  joint 
actuation  torques  and  joint  positions  is  known  and  controllable. 
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These  equations  were  written  in  symbolic  form  in  Matlab  [21].  The 
macromanipulator  tip  angular  velocities,  angular  accelerations,  and  translational 
accelerations  are  the  boundary  conditions  and  are  given  by: 


(3-28) 


These  were  used  in  equation  3-25  and  propagated  forward  to  each  link,  and  then 
equations  3-26  were  used  to  find  the  interaction  forces  and  torques  and  joint  actuation 
torques.  These  equations  were  developed  for  anthropomorphic,  spherical,  wrist,  and 
anthropomorphic/wrist  robot  configurations  and  many  are  included  in  Appendix  A.  The 
ability  of  this  method  to  predict  the  interaction  forces  and  torques  was  verified 
experimentally  with  a  six-degree-of  freedom  force/torque  sensor  mounted  at  the  base  of  a 
three  DOF  anthropomorphic  rigid  robot. 


3.4  Coupled  Macro/Micromanipulator  Model 
The  micromanipulator  is  considered  in  this  work  to  apply  the  interaction  forces  and 
torques  to  the  flexible  base.  The  flexible  manipulator,  given  by  equation  3-19,  is  now 
written  with  translational  and  rotational  effects  considered  separately.  The  generalized 
forces  applied  by  the  micromanipulator  are  given  by  3-18.  Here,  for  the  sake  of 
generality,  the  interactions  are  assumed  applied  directly  to  the  flexible  base.  In  this  case, 
the  flexible  manipulator  equations  of  motion  become: 
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(3-29) 
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where  Xf  represents  the  coordinates  governing  the  translational  motion  of  the  flexible  base 
and  0f  represents  the  coordinates  governing  the  rotational  motion  of  the  base.  The 
interactions  commanded  by  the  rigid  robot  motion  are  given  by  equations  3-27. 

These  are  rewritten  here  to  explicitly  to  identify  a  few  key  terms: 


F ',F=Bf(Oj0 +Nf(0,dfiJ)  +  Af(0)Xf +8^)6  f  +  Nfc(xf,xf,0f,df, 0,0 ) 
T,F=Brt(ejQ+Nz,(QjpJ)  +  AJd)xf+B,AT(j(ej6f+Nfxa{xf,xf,6I,ef,6,6) 

T=BT(e)e +NI(0,eidJ)+B'f(6)xf  +BTt0(djef +Njxf,xf,df,efe,e)  (3-30) 


Substituting  3-30  into  3-29  and  rearranging  yields  the  coupled  equations  of  motion: 


~M  +  Af{0 )  B„{9)  Bf(0) 
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Nl0(6,8iej)  +  Nr0c(xf,iLr8f,df,8,d) 

= 

0 

T 

The  goal  of  the  next  chapter  is  to  investigate  these  equations  of  motion  more 
thoroughly  and,  in  particular,  study  the  interactions  directly  controllable  by  the  rigid 
robot. 
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CHAPTER  IV 


INTERACTION  FORCES  AND  TORQUES  AND 
INERTIAL  SINGULARITIES 

4.1  Introduction 

This  chapter  addresses  the  interaction  forces  and  torques  acting  between  the  rigid 
robot  and  its  flexible  base.  Recall  these  are  given  by: 

F1F=Bf(d)6  +  Nf(6,0idj)  +  Cfm  +Nfc( q,q,M) 

hF  =BrO(6)0  +  NrO(O,0iej)  +  Ctom  +Nt0c(q,q,e,e )  (4-1) 

The  focus  of  this  chapter  is  the  directly  controllable  rigid  robot  effects,  or  those  terms  that 
are  only  functions  of  0  (the  first  two  terms  in  each  equation).  For  completeness,  the  more 
detailed  equations  of  motion  for  each  robot  may  be  found  in  Appendix  A 
(anthropomorphic,  spherical,  wrist,  and  anthropomorphic/wrist  robots).  The  notation  and 
coordinate  systems  used  are  shown  in  Figures  4-1  through  4-4.  The  robot  configurations 
are  defined  consistent  with  those  described  in  Sciavicco  and  Siciliano  [61]  for 
anthropomorphic  and  spherical  arm  robots  and  spherical  wrist  robots,  with  one  exception 
noted  in  Figure  4-1.  In  addition,  terminology  consistent  with  McGill/King  [47]  will  be 
used  where  centripetal  refers  to  accelerations  and  centrifugal  refers  to  forces. 

First,  the  inertia  forces  and  torques  are  discussed,  or  those  generated  by  accelerating 
the  links  of  the  rigid  robot.  An  important  part  of  this  work  involved  investigating 
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“inertial  singularities,”  or  regions  in  the  workspace  where  the  robot  loses  its  ability  to 
create  inertia  interactions  in  one  or  more  degrees  of  freedom.  These  are  important 
because  the  control  scheme  inverts  the  inertia  matrix,  which  presents  a  problem  when 
these  matrices  become  singular.  However,  these  singularities  also  represent  physical 
limitations,  so  it  is  important  to  understand  where  and  why  they  occur  and,  if  possible, 
devise  ways  of  avoiding  operation  in  or  near  these  regions. 

Next,  the  nonlinear  centrifugal  and  coriolis  forces  and  torques  are  discussed.  There 
are  regions  in  the  workspace  where  these  can  become  large  during  multi-DOF  actuation 
of  the  joints.  These  effects  are  driven  by  the  joint  space  configuration  of  the  robot  and 
the  joint  velocities.  In  most  configurations,  the  ratio  of  inertia  to  nonlinear  effects  is 
large.  However,  in  some  configurations  the  nonlinear  effects  can  interfere  with  the 
inertia  effects.  If  the  amplitude  of  joint  motion  is  limited  appropriately,  this  ratio  can  be 
improved.  The  remainder  of  the  interaction  terms  involve  combined  rigid  robot  and 
flexible  base  coordinates  and  are  not  discussed  further  here  other  than  to  note  they  are 
important  for  accurate  modeling.  Finally,  the  interaction  forces  and  torques  were 
developed  using  the  Newton-Euler  method  described  in  chapter  3.  However,  a  more 
efficient  approach  to  find  the  inertia  and  nonlinear  rigid  force  effects  is  presented  here. 

The  work  presented  in  this  chapter  leads  to  two  important  conclusions,  which  will  be 
used  in  the  development  of  the  control  scheme.  First,  since  the  inertia  effects  are 
functions  of  the  joint  configuration,  this  variation  in  performance  may  be  used  to  ensure 
the  robot  operates  in  joint  space  configurations  better  suited  for  inertial  damping.  This  is 
possible  because  there  are  normally  multiple  inverse  kinematic  solutions  corresponding 
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to  a  given  end  point  position  (provided  the  robot  is  not  operating  at  a  kinematic 
singularity).  A  performance  measure  to  predict  the  rigid  robot's  ability  to  create  effective 
multi-DOF  inertia  interactions  is  introduced  here.  It  can  be  used  to  choose  better  inverse 
kinematic  solutions  for  inertial  damping  and  will  become  an  important  part  of  the 
performance  index  discussed  in  Chapter  5.  Second,  if  the  joint  amplitudes  are  limited 
appropriately,  the  ratio  of  inertia/nonlinear  effects  can  be  improved.  The  amplitude  of 
motion  can  be  controlled  by  establishing  proper  limits  on  the  vibration  control  feedback 
gains.  The  control  scheme  and  feedback  gains  will  be  discussed  in  Chapter  5.  The 
adequacy  of  these  models  in  predicting  the  interactions  was  tested  on  a  three  DOF 
anthropomorphic  robot  with  a  six-axis  ATI  force/torque  sensor  mounted  to  its  base. 
These  results  will  be  presented  in  Chapter  7. 
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note  unconventional  definition  of  02 

Figure  4-1 

3  DOF  Anthropomorphic  Robot 
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4.2  Inertia  Forces  and  Torques  and  Inertial  Singularities 
The  inertia  forces  and  torques  are  functions  of  the  joint  configuration  of  the  robot  and 
joint  accelerations.  The  ability  of  three  typical  robots  to  generate  interactions  is 
examined  in  three  degrees  of  freedom  since  this  is  easily  visualized.  Using  the  notation 
defined  in  equation  4-1,  Bf  and  Bxo  are  inertia-like  matrices  but  they  are,  in  general,  not 
square,  symmetric,  or  positive  definite  (the  inertia  matrix  for  the  coupled  system,  Bx  from 
equation  3-27,  is).  These  matrices  are  particularly  important  for  two  reasons.  First,  the 
rigid  robot  needs  to  have  enough  inertia  to  apply  effective  interaction  forces  and  torques 
to  the  macromanipulator.  The  ratio  of  the  rigid  inertia  to  flexible  inertia  effects  becomes 
an  important  part  of  the  performance  index.  Second,  there  are  locations  in  the  workspace 
where  these  matrices  become  singular,  which  presents  a  problem  since  they  are  inverted 
in  the  control  scheme.  However,  the  more  important  consideration  is  that  these  “inertial 
singularities”  represent  physical  limitations  in  that  an  inertia  force  or  torque  cannot  be 
created  in  one  or  more  degrees  of  freedom. 

4.2.1  Performance  Measure  for  Inertial  Damping 

The  following  performance  measure  provides  a  quick  and  accurate  measure  of  the 
ability  of  the  rigid  robot  in  generating  effective  interaction  forces  and  torques  and 
assesses  its  variation  throughout  the  workspace: 

s;(«)S/(#)|,|j),,'0(«)B,0(«)|  (4-2) 
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Alternately  the  combined  ability  of  the  robot  to  generate  interaction  forces  and  torques 
may  be  evaluated  by  assessing: 


~Bfm ' 


B  = 


(4-3) 


First,  it  is  important  to  note  a  few  points  about  these  matrices.  They  are  real  but  not, 
in  general,  square,  or  symmetric  and  thus  will  not  have  real  eigenvalues  [73].  However, 

T 

B  B  will  always  be  symmetric  and  its  determinant  will  be  positive.  Singular  value 
decomposition  is  a  related  technique  noted  for  its  usefulness  in  determining  how  near  a 
matrix  comes  to  becoming  singular.  It  has  also  been  noted  that  the  use  of  only  the 
minimum  and  maximum  singular  values  is  overconservative  [23].  Thus,  the  determinant 
of  B  B,  which  is  the  product  of  the  singular  values,  was  chosen  for  the  performance 
measure.  In  addition,  although  this  dissertation  does  not  specifically  address  cases  of 
underactuation  or  redundancy,  the  proposed  measure  may  be  extended  to  those  cases 
since  there  is  no  requirement  that  the  matrices  be  square.  Finally,  the  determinant  of  a 
matrix  can  much  more  easily  be  evaluated  than  eigenvalues  or  singular  values,  which  is 
particularly  important  if  the  performance  measure  will  be  used  in  real-time.  This 
measure  not  only  provides  an  indication  of  how  these  effects  vary  throughout  the 
workspace,  but  also  shows  regions  where  full  multi-DOF  inertial  damping  capability  is 
not  possible.  The  goal  here  is  to  use  this  performance  measure  to  choose  robot  joint 
space  locations  where  the  inertia  effects  are  large,  whenever  possible. 
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4.2.2  Interaction  Force  Performance 


Consider  the  three  degree  of  freedom  anthropomorphic  robot  shown  in  Figure  4-1. 
Note  this  is  a  special  case  where  three  joints  create  interactions  in  three  degrees  of 


freedom.  Hence  the  inertia  force  matrix  is  square  and  given  by: 


$1  (Ac2  +  Bc23) 

-c1(As2+Bs23) 

~Bcxs23 

II 

c,(Ac2  +  Bc23 ) 

-s1(As2+Bs23) 

~Bsxs23 

(4-4) 

0 

Ac2  +  Bc23 

Bc23  _ 

where: 


A=m2r2+ni3a2 

B=ni3r3 

nij=mass  of  link  i 
aj=length  of  link  i 

rj=distance  to  the  center  of  gravity  (CG)  of  link  i 


The  determinant  allows  an  evaluation  of  the  singularity  points  and  is  given  by: 

\Bf\  =  -ABs3  ( Ac2  +  Bc22  )  (4-5) 

The  matrix  becomes  singular  whenever  S3=0  or  Ac2+Bc23=0. 

The  variation  in  force  performance,  as  quantified  by  the  performance  measure  in 
equation  4-2,  is  shown  in  Figure  4-5  over  a  reduced  range  of  joint  motion  in  order  to  help 
clarify  the  presentation.  Cases  1  and  2  refer  to  specific  regions  in  the  workspace  that  will 
be  referred  to  later. 
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Force  Performance  -  Anthropomorphic  Robot 


Figure  4-5 

Force  Performance  -  3  DOF  Anthropomorphic  Robot 


The  inertia  matrix  for  the  spherical  robot  and  its  determinant  are  given  by: 


AsCl  BsS\S2 

Bff2 

~4Sl  +BsClS2 

BsS\C2 

0 

~BsS2 

W3C2  _ 

Bf  =-n%s2r? 

(4-6) 

where: 


As=m2r2+m3d2 

Bs=m3r3  (note  r3  is  not  constant) 
dj=length  of  link  i 
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Its  variation  in  performance  is  shown  in  Figure  4-6. 


Force  Performance  -  Spherical  Robot 


Figure  4-6 

Force  Performance  -  3  DOF  Spherical  Robot 


The  equivalent  matrix  for  the  three  DOF  wrist  is  given  by: 


AwS4‘S5 


0 


4/^5 

~AvSS 


o 

0 

0 


(4-7) 


where: 


Aw=m5r5+(d5+r6)m6 
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Its  variation  in  performance  is  shown  in  Figure  4-7. 


Force  Performance  -  Wrist  Robot 


Figure  4-7 


Force  Performance  -  3  DOF  Wrist  Robot 

The  regions  with  very  low  values  represent  locations  in  the  workspace  where  the 
robot  cannot  produce  inertia  forces  in  one  or  more  degrees  of  freedom.  These  will  be 
termed  “inertial  singularities.”  These  singularities  consist  of  some  of  the  kinematic 
singularities  plus  additional  dynamically  singular  configurations.  The  two  general  cases 
are  annotated  on  Figures  4-5  through  4-7.  The  singularities  are  driven  by  the  columns  of 
Bf  when  the  matrix  contains: 

1)  Linearly  dependent  columns,  which  indicate  that  the  inertia  forces  created  by  two  or 
more  links  are  parallel.  For  example,  this  singularity  occurs  for  the  anthropomorphic 
robot  when  the  last  two  links  are  aligned.  This  also  corresponds  to  a  kinematic 
singularity,  when  the  velocities  generated  by  the  two  links  are  parallel.  For  the 
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spherical  robot,  this  occurs  when  joint  2  is  at  0°  or  180°,  when  both  links  1  and  2 
create  velocities  (and  inertia  forces)  in  the  horizontal  direction.  All  of  the  three  DOF 
spherical  robot  singularities  are  the  same  as  the  kinematic  singularities. 

2)  A  column  of  zeros,  which  indicates  a  location  in  the  workspace  where  the  motion  of  a 
joint  cannot  create  any  inertia  interaction  forces.  This  singularity  occurs  when  the 
CG  of  the  robot  is  aligned  along  an  axis  of  rotation.  For  the  anthropomorphic  robot, 
this  occurs  when  the  system  CG  is  aligned  along  the  z-axis,  so  link  1  can  produce  no 
interaction  inertia  forces.  In  the  spherical  robot,  this  will  occur  if  link  3  is  aligned 
along  the  axis  of  joint  2,  which  renders  a  middle  column  of  zeros.  This  singularity 
can  be  avoided  by  requiring  some  minimum  length  to  link  three.  These  are,  in 
general,  different  from  the  kinematic  singularities.  The  inertial  singularities  depend 
on  the  location  of  the  CG  of  the  system.  If  it  is  known  where  these  occur,  they  can  be 
avoided  by  not  operating  in  those  joint  space  configurations. 

In  the  case  of  the  anthropomorphic  and  spherical  robots,  link  1  has  no  effect  on  the  inertia 
force  performance  because  it  cannot  affect  the  CG  of  the  system. 

Given  the  configuration  shown  in  Figure  4-3,  the  wrist  is  always  singular  in  three 
degrees  of  freedom.  Its  last  link  cannot  create  any  inertia  interaction  forces  since  its  axis 
of  rotation  is  along  the  main  link  of  the  robot.  In  addition,  only  the  orientation  of  joint  5 
affects  the  inertia  forces  since  it  is  the  only  joint  that  affects  the  CG  of  the  system.  It  is 
also  interesting  to  note  the  wrist  singularities  occur  when  the  arm  is  straight  down  or 
straight  up  (05=0°  or  180°).  This  is  a  combination  of  cases  1  and  2  since  these  are 
kinematic  singularities  and  the  CG  is  aligned  along  joint  1.  Note  the  extremely  small 
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force  magnitudes  generated  by  the  wrist,  indicating  its  use  alone  would  not  be  very 
effective  at  creating  interactions. 

It  is  also  worth  mentioning  another  type  of  singularity,  which  corresponds  to  a  row  of 
zeros  in  the  matrix  or  two  parallel  rows.  This  singularity  occurs  in  conjunction  with  one 
of  the  other  singularity  configurations  discussed  above.  For  example,  if  the  CG  of  the 
anthropomorphic  robot  is  oriented  along  the  axis  of  rotation  of  joint  1  (case  2),  an  inertia 
force  cannot  be  produced  in  the  out-of-plane  direction.  In  this  case,  two  of  the  rows  can 
become  parallel  or  one  can  become  a  row  of  zeros,  depending  on  0i.  Two  other  types  of 
row  singularities  can  occur  in  conjunction  with  the  kinematic  singularity  at  03=0°  (case 
1).  When  joint  2  is  at  90°  (straight  down)  and  joint  3  is  at  0°,  no  inertia  forces  can  be 
produced  in  the  z  direction  and  the  third  row  becomes  a  row  of  zeros.  Finally  the 
orientation  at  02=0°,  03=0°  can  result  in  a  row  of  zeros  in  either  the  x  or  y  direction, 
depending  on  0j.  These  are  easily  identified  using  the  proposed  performance  measure 
defined  in  equation  4-2  and  serve  to  further  verify  the  singularities  are  driven  by  the 
columns  of  the  inertia  matrices. 
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4.2.3  Interaction  Torque  Performance 


The  interaction  torque  matrices  are  given  by  equations  4-8  through  4-10: 
Anthropomorphic  Robot: 

^roO>l)  —  £|  {  2c..y23| c3  (E  + ./)  +  Ho  3  ]  +  (E  +  J^(s2c2  +  53c3)  +  B(ci2s3  —  Dc23)  +  ( G  —  E ).s\c2  —  A Ei\ } 
5r0(l,  2)  =  s,  {5(2a2c3  +  Ds23)  +  ADs2  +(E  +  H  +  K)} 

Br0(h  3)  =  5,  {B(a2c3  +Ds23)  +  E  +  I3J 

Br0(2,\)  =  s]{-2c2s23[c3(E  +  J)  +  Ba2]  +  (E  +  J)(s2c2  +s3c3)  +  B(a2s3 -Dc23)  +  (G-K)s2c2-ADc2} 
Bt0(2 , 2)  —  — Cj  {.Z?(2n2c3  +  Ds23)  +  AL)s2  +  ( E  +  H  +  /C ) } 

5r0(2,3)  =  -c,  {5(a2c3  +  Ds23)  +  {E  +  I3zz)} 

BtQ  (3, 1)  =  2c2c23  [c3(£  +  J)  +  Ba2  ]-(E  +  J)(c22  +  c23)+(K-G)c22+E+L 

Bt0  (2,2)-  Bt0  (3, 3)  =  0  (4-8) 

where: 


D— do+dj 
E=m3r32 
G— Exx^Eyy 

H=l2zz+l3  zz 

J=-(l3xx”l3yy) 

K=m2r22+ni3a22 
L_Ilyy+l2xx+l3yy 
do^length  of  link  0  (inert  link) 

Ijkk=moment  of  inertia  of  link  j  about  the  k  axis 


Spherical  Robot: 


j  Bs  G  j  )c,c2s2  +  B  S(a]cls2  +  q2sxc2)  Asoxsx 
Br  oC1-2)  =  s  +  fs)h  +  B  S(a,sxc2  -  a2cxs2 ) 

5r0(l,3)  =  m  3(axs]s2  +  a2cxc2) 

B  x  o(2>l)  (  BI  s  E  s  G  s) s xs 2c 2  +  B  s  (axsxs2  g  2c xc 2 )  +  A  s  ci  xc  x 

Bt  o(2,2)  =  (£s  +  FJc,  -  B  s(alclc2  +  a2sxs2) 

B t0(2,3)  =  -m3(axcxs2  -  a2sxc2 ) 

B  to  (3,1)  =  E,  +  /lw  +  G,  +  +  (ff,  -  -  G  s)c\ 

■®ro(3j2)  =  —  B  sa  2c  2 

Br  o  (3,3)  =  -  m3a2s2  (4-9) 
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where: 


Es=m3r32 
F  s-l2yy+l3yy 
Gg-fexx+fexx 

HS“l3zz^"l2  zz 

Ks=m3a22+m2r22 


Wrist  Robot: 


Bxo  (h  t)  s5  { (Kw  +  Ey  4-  Lw)cacs  +  AwdAcA  +  dwc6{sAs6  c4c5cg)J 

Bx0(y, 2)  =  ~(MW  +  EW  +  Lw)sA  -  AJasacs  -  Jwc6{sAc6  +  cAc5s6) 

-®ro(h3)  =  -^6zzC455 

5r0(2,l)  =  S5  {~(k  +  Ew  +  A>  4C5  “  AJaS4  +  J«C6  (<V6  -  W«)} 

Bl0 (2, 2)  —  ( A +  isw  +  L  )cA  +  AwdAc4c5  +  Jwc6(cAc6  —  sAc5s6) 

Br0{2,3)  =  I6zzsas5 

BA  3,1)  =  -s52(*w  +  £w  +  4)  - -Vk2 
Bt0O,2)  =  Jws5s6c6 

Bt0(3,3)  =  I6zzcs  (4-10) 


where: 

E,v=m6r6 

J w  hyy'hxx 

Kw=m5r52+m6d52 

Lw“  I5xx”l5zz+l6yy_l6zz  ^  ^TH^r^cls 

Mw-l5yy+l6xx+2m6r6d5 

El  w . “  1 4  y  i  5  x  x  ^  1 6  vy^2  m  (,  r^  d  5 

The  inertia  torque  performance  of  the  three  DOF  anthropomorphic  robot  can  be  seen 
in  Figure  4-8  by  evaluating  the  torque  performance  measure  defined  in  equation  4-2. 
However,  the  inertia  torques  created  by  accelerating  joints  2  and  3  are  always  parallel  so 
this  evaluation  was  made  for  joints  1  and  2  only  (first  two  columns  of  the  matrix).  This 
highlights  one  advantage  of  using  this  performance  measure  instead  of  the  determinant  of 
the  matrix,  because  performance  of  the  robot  in  reduced  degrees  of  freedom  can  still  be 
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evaluated.  The  inertia  torque  performance  for  the  spherical  robot  is  shown  in  Figure  4-9 
and  the  wrist  torque  performance  is  shown  in  Figure  4-10. 


Torque  Performance  -  Anthropomorphic  Robot 


Figure  4-8 

Torque  Performance  -  2  DOF  Anthropomorphic  Robot 


The  torque  singularities  are  much  less  intuitive  than  the  force  singularities.  This  is 
further  complicated  by  the  complexity  of  the  interaction  torque  equations,  even  when 
simplified  as  much  as  possible  (nor  does  the  determinant  simplify  into  a  nice  form). 
However,  they  still  occur  in  the  same  two  general  cases:  either  when  one  or  more 
columns  becomes  parallel  (case  1)  or  one  or  more  columns  of  zeros  occur  in  the  BTo 
matrix  (case  2).  For  the  anthropomorphic  robot,  the  torque  singularities  occur  near  the 
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same  regions  as  the  case  2  force  singularities.  However,  in  this  case  the  torques  created 
by  accelerating  the  CG  of  the  robot  are  cancelled  by  accelerating  the  inertia  of  a  link, 
which  results  in  a  column  of  zeros.  The  spherical  torque  singularities  occur  when  the 
torques  created  by  all  three  links  become  parallel. 


Torque  Performance  -  Spherical  Robot 


Figure  4-9 

Torque  Performance  -  3  DOF  Spherical  Robot 


Torque  Performance  -  Wrist  Robot 


Figure  4-10 

Torque  Performance  -  3  DOF  Wrist  Robot 


The  wrist  torque  performance  is  again  only  a  function  of  05  and  of  relatively  small 
magnitude,  reiterating  the  assertion  that  it  is  unlikely  to  be  useful  alone  in  creating 
effective  interactions.  It  also  has  a  large  region  of  very  small  torque  performance  from 
1OO°<05<18O°.  This  occurs  because  the  last  link  is  oriented  up  towards  the  first  link, 
which  reduces  the  effective  inertia  of  links  1  and  3.  However,  the  wrist  can  be  useful 
when  added  to  the  last  link  of  an  existing  robot.  First,  the  full  inertia  matrix,  B(0), 
becomes  6x6  and  could  then  be  directly  inverted  in  the  control  scheme  to  provide  full 
six  degree  of  freedom  inertial  damping  capability.  Another  option  is  to  use  the  additional 
inertia  of  the  wrist  to  increase  the  damping  effectiveness  of  the  base  robot.  The  wrist 
could  then  be  used  to  provide  final  positioning  or  to  give  desired  orientation  of  the  end 
effector. 
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4.2.4  Combined  Performance 


The  combined  force  and  torque  performance  of  the  robots  can  be  evaluated  by  using 
equation  4-3.  It  is  important  to  note  that,  even  if  the  interaction  forces  or  torques  are 
prescribed  independently,  commanding  one  will  always  command  the  other.  Thus,  it 
becomes  important  to  be  able  to  evaluate  the  combined  force  and  torque  performance  of 
the  robot.  The  combined  performance  plots  for  the  anthropomorphic  and  spherical  robots 
are  shown  in  Figures  4-11  and  4-12.  In  these  cases,  the  interaction  inertia  matrix 
becomes  6x3,  but  the  proposed  performance  measure  still  allows  evaluation  of  the 
combined  force  and  torque  performance. 


Combined  Performance  -  Anthropomorphic  Robot 


Case  1  Force  Q  0 
Singularities  3 


200  100 


20 

Case  2  Force 
and  Torque 
Singularities 


Figure  4-11 

Combined  Force  and  Torque  Performance  -  3  DOF  Anthropomorphic  Robot 
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Combined  Performance  -  Spherical  Robot 


Case  1  Force  Singularities 
Figure  4-12 


Combined  Force  and  Torque  Performance  -  3  DOF  Spherical  Robot 

For  the  two  robot  configurations  shown  here,  the  addition  of  the  torque  effects  makes 
little  difference  in  the  overall  performance.  As  can  be  seen  in  Figure  4-11,  the 
anthropomorphic  robot  inertial  force  singularities  are  still  clearly  apparent  and  the  overall 
shape  of  the  performance  measure  remains  the  same.  For  the  spherical  robot,  the  inertia 
forces  are  even  more  dominant  and  the  torque  singularities  are  practically  eliminated 
from  the  plot.  The  torque  effects  also  slightly  improve  the  performance  around  the  case  1 
force  singularities.  The  ideal  situation  would  be  to  fully  prescribe  both  forces  and 
torques  for  six  DOF  base  vibration  damping.  However,  these  plots  indicate  that  if  only 
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the  forces  or  torques  are  to  be  prescribed  it  is  more  important  to  consider  the  interaction 
forces. 


4.3  Nonlinear  Rigid  Interaction  Force  and  Torque  Effects 
This  section  investigates  the  nonlinear  interaction  forces  and  torques  expecting  during 
multi-degree  of  freedom  inertial  damping.  Inertial  damping  has  been  shown  previously  in 
limited  degrees  of  freedom  and  with  robots  oriented  in  specific  configurations.  The 
single  link  case  is  relatively  straightforward  since  the  acceleration  of  the  CG  of  the  link 
and  centripetal  acceleration  will  be  perpendicular  for  rotational  joint  motion  [47].  This 
will  not  be  the  case  with  multiple  joint  actuation,  so  it  becomes  more  important  to  address 
these. 

The  interaction  forces  and  torques  directly  controllable  by  the  rigid  micromanipulator 
are  given  by: 


VIP=Bf(6) 


1 

1 _ 

"44 

'4' 

4 

+  NRf(0) 

44 

+  NCf(6) 

42 

i 

_ i 

i 

_ 1 

- 1 

4 

1 

NJ 

_ 1 

1 

NJ 

_ 1 

*5 

II 

o 

4 

+  NRtO(0) 

44 

+  NCtO(0 ) 

42 

4. 

i 

<N 

_ \ 

i 

1 _ 

+  GA6) 


+  GtO(0) 


(4-11) 


Here  coriolis  and  centrifugal  effects  have  been  written  separately.  Gravity  effects  are  not 
further  discussed  here  since  they  are  not  dynamic  effects.  In  addition,  in  Chapter  5 
rationale  will  be  presented  which  allows  the  assumption  to  be  made  that  the  matrices 
governing  the  interactions  may  be  linearized  about  an  operating  point. 
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The  nonlinear  rigid  effects  are  functions  of  the  manipulator  position  and  joint 


velocities.  The  anthropomorphic  rigid  coriolis  force  matrix  is  given  by: 


NRf  = 


2s1(As2  +Bs23) 
-2c1(As2  +2Bs23) 
0 


2Bs,s23 
-  2Bc{s23 
0 


-  2Bcxc23 
-2Bsxc23 

-  2  Bs23 


(4-12) 


The  anthropomorphic  rigid  centrifugal  force  matrix  is  given  by: 


~cx(Ac2+Bc23) 

-cx(Ac2  +  Bc23) 

~Bcxc23 

NCf  = 

-sx{Ac2+Bc23) 

-sx(Ac2+Bc23) 

—  BSyC23 

(4-13) 

0 

-(As2+Bs23 ) 

1 

£ 

1 

First,  the  variation  in  the  nonlinear  effects  due  to  joint  position  is  investigated.  Next,  the 
effect  of  joint  amplitudes  on  these  effects  will  be  discussed. 

Assuming  harmonic  base  vibration  of  mode  i  of  the  flexible  base,  the  base  motion  and 
prescribed  interaction  forces  and/or  torques  will  be  harmonic  and  take  the  form 
(justification  for  this  will  be  shown  in  Chapter  5): 


xt  =Xjsmo)it 

Fjf/Tif  =  cos  oaf  (4-14) 


Kj  can  be  used  to  adjust  the  amount  of  damping  added  to  the  system.  From  equation  4- 
11,  the  interaction  effects  are  controlled  by  joint  postions,  velocities,  and  accelerations. 
If  any  one  of  these  are  commmanded  harmonically,  the  others  will  also  be  commanded 
harmonically. 

First,  consider  the  nonlinear  forces  generated  by  the  anthropomorphic  robot  during 
multi-degree  of  freedom  harmonic  joint  motion  (Figures  4-13  and  4-14).  These  plots 
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were  generated  assuming  all  joints  are  actuating  simultaneously  and  at  constant 
amplitudes  given  by: 

0  =  15°  (— — )  cos(l  .4  *  2nt) 

180° 

6  =  -2.3sin(l  .4  *  2nt) 

9  =  -20  cos(l  .4  *  2nt)  (4-15) 

Harmonic  motion  at  1 .4  Hz  was  chosen  because  it  is  the  fundamental  mode  of  vibration 
of  a  long,  flexible  link.  This  is  representative  of  a  fundamental  mode  of  a  flexible 
macromanipulator,  i.e.  lightly  damped,  low  frequency  harmonic  vibration.  The  inertia 
forces  generated  under  these  same  circumstances  can  be  seen  in  Figure  4-15.  Note  these 
are  not  the  same  comparisons  as  the  figures  in  Section  4.2. 

Nonlinear  Coriolis  Forces  -  Anthropomorphic  Robot 


Figure  4-13 

Anthropomorphic  Robot  Coriolis  Forces 
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Nonlinear  Centrifugal  Forces  -  Anthropomorphic  Robot 


250  ^ 


Figure  4-14 

Anthropomorphic  Robot  Centrifugal  Forces 


Inertia  Forces  -  Anthropomorphic  Robot 


200  100 


Figure  4-15 

Anthropomorphic  Robot  Inertia  Forces 
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Although  this  initial  analysis  may  be  overconservative  since  it  does  not  consider  the 
effect  of  decaying  amplitudes  or  phasing,  it  does  bring  to  light  several  important 
considerations.  First,  the  magnitude  of  the  inertia  forces  is  much  larger  than  the 
nonlinear  forces.  Second,  coriolis  forces  are  largest  in  near-singularity  regions  (case  2  in 
Figure  4-5).  Thus,  the  coriolis  forces  are  not  a  great  concern  because  operation  in  these 
regions  will  be  avoided  by  using  the  performance  index.  However,  the  centrifugal  forces 
are  largest  around  the  kinematic  singularities  (case  1  in  Figure  4-5)  and  where  the  inertia 
forces  are  largest,  as  shown  in  Figure  4-15.  In  fact,  the  centrifugal  forces  are  maximum 
at  the  singularity  point  02=0°,  03=0°.  Operation  exactly  at  the  singularity  points  will  be 
avoided,  but  operation  around  them  may  be  required,  especially  if  large  inertia  forces  are 
desired. 

Since  the  anthropomorphic  robot  is  always  singular  in  three  DOF  inertia  torque 
performance,  the  spherical  robot  was  chosen  to  study  the  nonlinear  torque  effects.  The 
spherical  robot  coriolis  and  centrifugal  torque  matrices  are  given  by  equations  A-37  and 
A-38  in  Appendix  A.  Note  the  third  link  is  prismatic,  so  r3  is  a  variable.  Figures  4-16 
and  4-17  show  the  variation  in  nonlinear  torque  performance  throughout  the  joint 
workspace  and  Figure  4-18  shows  the  inertia  torques.  These  plots  indicate  that  the 
coriolis  torques  are  greatest  in  joint  configurations  where  the  inertia  torque  effects  are 
largest.  The  centrifugal  torques  are  largest  in  regions  where  the  inertia  torques  are  small, 
indicating  they  will  interfere  less  with  the  inertia  torques.  Again,  the  magnitude  of  the 
inertia  torques  is  greater  than  the  nonlinear  torques,  but  not  to  the  same  extent  seen  for 
the  anthropomorphic  robot  forces.  Although  these  results  are  not  presented  here,  the 
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anthropomorphic  robot  also  has  a  much  larger  ratio  of  inertia/nonlinear  torques  than  the 
spherical  and  wrist  robots.  This  occurs  primarily  because  the  last  link  of  the  spherical 
robot  is  prismatic  and  accelerating  it  does  not  provide  the  additional  rotational  inertia 
effects.  In  general,  for  both  robots  the  centrifugal  torques  become  large  near  the  inertial 
singularity  torque  regions,  while  the  coriolis  torques  become  more  of  a  concern  in  regions 
where  the  inertia  torques  are  large. 


Nonlinear  Coriolis  Torques  -  Spherical  Robot 


Figure  4-16 

Spherical  Robot  Coriolis  Torques 
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Nonlinear  Centrifugal  Torques  -  Spherical  Robot 


Figure  4-17 

Spherical  Robot  Centrifugal  Torques 


Inertia  Torques  -  Spherical  Robot 


Figure  4-18 

Spherical  Robot  Inertia  Torques 
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From  these  initial  studies,  Matlab  simulations  were  built  for  the  anthropomorphic 
robot  to  further  investigate  the  relative  magnitudes  of  the  force  and  torque  effects.  These 
were  simulated  by  commanding  the  joint  accelerations  proportional  to  the  velocities 
calculated  from  assumed  harmonic  motion  of  the  base  in  three  degrees  of  freedom.  For 
example,  the  anthropomorphic  robot  was  tested  in  a  configuration  expected  to  provide  a 
large  inertia/nonlinear  force  ratio,  as  predicted  by  Figures  4-5,  4-13,  and  4-14  (0°,  20°, 
70°).  The  ratio  of  the  inertia  to  nonlinear  effects  is  large,  as  can  be  seen  in  Figure  4-19 
(in  these  plots,  the  inertia  and  total  force  traces  nearly  overlay  and  are  indistinguishable). 


Time  (s) 

Figure  4-19 

Anthropomorophic  Interaction  Forces,  Large  Inertia  Effects 


The  same  comparison  is  shown  in  Figure  4-20  with  the  robot  in  the  configuration  [0°, 
85°,  10°],  which  is  a  region  where  the  coriolis  forces  are  expected  to  be  large  as  well  as 
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near  a  case  2  inertial  singularity.  The  reduced  ability  of  the  robot  to  produce  inertia 
forces  is  seen  in  the  y  direction  as  well  as  the  increased  coriolis  effects  (coriolis  and  total 
traces  are  nearly  overlaid  in  the  second  subplot).  Also,  note  the  increased  centrifugal 
forces  in  the  z  direction  (centrifugal  and  total  traces  are  overlaid  in  the  third  subplot). 
Since  both  joints  are  nearly  straight  out,  the  centrifugal  forces  due  to  joints  2  and  3  act 
primarily  in  the  z  direction.  This  scenario  is  near  an  inertial  singularity  region,  which 
will  be  avoided  if  possible.  The  nonlinear  effects  give  even  more  reason  to  avoid  these 
joint  space  configurations. 
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Figure  4-20 

Anthropomorphic  Interaction  Forces,  Near  Singularity  Case  2 
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Figure  4-14  predicted  large  centrifugal  forces  in  a  region  with  expected  large  inertia 
forces.  The  same  comparison  is  shown  in  Figure  4-21  in  the  configuration  [0°,20°,-10°], 
shown  in  Figure  4-22.  In  this  case,  the  inertia  forces  act  primarily  in  the  y  direction, 
while  the  nonlinear  centrifugal  forces  all  align  primarily  along  the  x-axis.  The  ratio  of 
the  inertia  to  nonlinear  forces  becomes  smaller  in  the  x  direction,  which  results  in  more 
nonlinear  effects  in  the  total  force  trace.  This  is  clearly  seen  in  the  top  figure  where  the 
centrifugal  (dashed  line)  effects  are  picked  up  in  the  total  force  trace. 
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Figure  4-21 

Anthropomorphic  Interaction  Forces,  Large  Centrifugal  Effects 
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4.3.1  Amplitude  Effects 

During  active  damping  (this  will  be  discussed  more  in  Chapter  5),  the  joints  will  be 
actuating  harmonically  to  damp  the  base  vibration.  The  effect  of  the  amplitude  of  the 
harmonic  joint  motion  has  not  yet  been  considered.  As  an  example,  consider  prescribing 
harmonic  joint  accelerations: 

0  =  A  cos  cot  (4-16) 

The  joint  velocities  will  be: 

A 

0  =  — sin  cot  (4-17) 

co 
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The  ratio  of  the  joint  accelerations,  which  directly  affect  the  inertia  effects,  to  the  square 
of  the  joint  velocities,  which  directly  affect  the  centrifugal  effects  and  occur  at  the  first 
harmonic  of  the  fundamental  frequency,  is  given  by: 


ft  _  A  cos  cot 

W~~4^ 

(—r)  sin2  cot 
co 


The  ratio  of  their  amplitudes  becomes: 


(4-18) 


(4-19) 


Alternately,  if  two  joints  are  actuating,  the  link  accelerations  and  velocities  are  given  by: 

ft,  =  Al  cos  coxt  ft,  =  Ai  cos  co2t 
A  ■  A, 

ft,  =— sin  co.t  ft2=— sinoJ  (4-20) 

cox  co2 

The  ratio  of  the  joint  accelerations,  which  drive  the  inertia  effects,  to  the  product  of  joint 
velocities,  which  drive  the  coriolis  effects,  becomes: 


^1  _  coxco 2 
¥ 2  -  A2 


(4-21) 


The  frequencies  of  actuation  will  be  driven  by  the  natural  frequencies  of  the 
macromanipulator,  which  are  not  controllable.  However,  the  ratio  is  also  inversely 
proportional  to  the  amplitude  of  motion,  which  is  controllable.  This  issue  is  addressed 
more  rigorously  in  Chapter  5,  where  the  joint  amplitudes  are  related  to  the  feedback  gains 
and  appropriate  gain  limits  established.  By  limiting  the  amplitude  of  motion  of  each 
joint,  both  of  the  nonlinear  effects  will  be  reduced. 
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For  example,  consider  the  anthropomorphic  robot  in  a  configuration  of  [0°,  75°,  10°]. 
The  second  link  is  moved  slightly  from  the  configuration  used  in  Figure  4-20  so  the  robot 
is  not  as  near  the  inertial  singularity  region.  The  inertia,  coriolis,  centrifugal,  and  total 
forces  are  shown  in  Figure  4-23.  The  top  two  plots  show  the  interactions  due  to  all  three 
joints  actuating  with  constant  amplitudes  of  approximately  23°.  The  bottom  two  plots 
show  the  forces  when  the  joint  amplitudes  are  reduced  in  half.  While  this  results  in  lower 
inertia  forces,  it  also  has  the  effect  of  improving  the  ratio  of  the  inertia/nonlinear  effects. 


Figure  4-23 

Effect  of  Reducing  Joint  Amplitudes  on  Nonlinear  Effects 
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4.4  CG  Method  to  Identify  Critical  Interaction  Force  Effects 
This  chapter  concentrated  on  the  interaction  effects  directly  controllable  by  the  rigid 
robot,  which  are  the  inertia  and  nonlinear  rigid  effects.  The  method  originally  used  to 
develop  the  force  equations  is  the  Newton-Euler  method  described  in  Section  3.3. 
However,  a  much  more  efficient  method  may  be  used  to  develop  the  interaction  forces 
directly  controllable  by  the  rigid  robot.  This  made  developing  the  six  DOF 
anthropomorphic/wrist  force  equations  manageable  and  was  the  method  used  to  derive 
equations  A-48  through  A-50  in  Appendix  A. 

The  micromanipulator  does  not  have  a  fixed  point,  so  the  interaction  forces  acting 
between  each  link  are  found  by  summing  the  moments  about  the  CG  of  each  link. 
Alternatively,  the  moments  may  be  summed  about  the  CG  of  the  overall  robot.  The 
position  vectors  to  the  CG  of  each  robot  are  given  in  Appendix  A.  Let: 

*cg  =  ^ ccfi  (4-22) 

The  interaction  forces  are  given  by: 

F»4w«»)  (4-23) 

at 

where  M  is  the  total  mass  of  the  rigid  robot.  The  inertia  and  nonlinear  rigid  effects, 
defined  in  Equation  4-1,  are  given  by: 

Bf  =MJcg 

Nf=MJcc9i  (4-24) 
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where  Nf  represents  combined  coriolis  and  centrifugal  forces.  If  desired,  the 
macromanipulator  inertia  effects  may  be  added  by  including  the  base  direction  cosine 
matrix,  which  is  defined  and  discussed  in  Book  [8],  Lew  [34],  and  Senda  [62]. 


rCG  —  *CGr 


1  ~0.  0 „ 


9, 


1  9 ' 


-e,  o,  l 


lCGr 


Bf  +  Af  —  JCG 


(4-25) 


where  rcGr  is  the  position  vector  (from  the  base)  to  the  CG  of  the  rigid  robot  and  0X>  0y, 
and  0Z  are  the  small  base  rotations  about  the  x,  y,  and  z  axis. 


4.5  Conclusions 

The  interaction  forces  and  torques  will  be  used  in  the  control  scheme  to  add  damping 
to  the  flexible  base.  At  this  point,  some  general  conclusions  should  be  made  about  the 
interaction  effects,  which  will  be  used  to  develop  the  vibration  controller  and  gain  limits 
discussed  in  Chapter  5. 

1)  The  nonlinear  and  inertia  effects  are  driven  by  two  factors:  the  configuration  of  the 
robot  and  joint  accelerations  and  velocities.  The  inertia  performance  measure  introduced 
in  equations  4-2  and  4-3  and  plotted  in  Figures  4-5  through  4-12  can  be  used  to  choose 
inverse  kinematic  solution(s)  best  suited  for  inertial  damping.  As  an  example,  consider 
the  anthropomorphic  robot  in  the  two  configurations  shown  in  Figure  4-24.  The  left 
configuration  shows  a  very  poor  configuration  for  inertial  damping  (case  2  inertial 
singularity),  while  the  alternate  inverse  kinematics  solution  shown  on  the  right  will 
provide  much  better  inertia  force  performance. 
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Figure  4-24 

Alternate  Inverse  Kinematic  Configurations  for  Anthropomorphic  Robot 


On  the  other  hand,  consider  the  configuration  shown  in  Figure  4-22,  which  resulted  in 
large  centrifugal  forces.  The  alternate  inverse  kinematics  solution  would  not  provide  an 
improvement  in  performance  in  this  case  since  the  problem  results  from  a  combination  of 
effects  created  by  all  of  the  joints,  which  would  occur  in  either  configuration. 

2)  In  general,  the  coriolis  forces  are  largest  near  inertial  force  singularity  regions, 
while  the  centrifugal  forces  can  become  large  in  regions  where  inertia  forces  are  large. 
The  centrifugal  torques  are  largest  near  inertial  torque  singularity  regions,  while  the 
coriolis  torques  are  more  of  the  concern.  There  are  configurations,  however,  where  one 
or  both  can  interfere  with  the  ability  of  the  robot  to  create  effective  inertia  forces  or 
torques. 

3)  The  inertia  effects  are  functions  of  the  joint  accelerations,  while  the  nonlinear 
effects  are  functions  of  the  joint  velocities.  The  amplitude  of  joint  motion  directly 
influences  the  ratio  of  the  inertia  to  nonlinear  effects,  as  shown  in  equations  4-19,  4-21 


and  Figure  4-23.  The  relationship  between  the  joint  amplitudes  and  feedback  gains  will 
be  developed  in  the  next  chapter,  and  gain  limits  established  to  ensure  this  ratio  remains 
favorable  for  inertial  damping. 

When  the  performance  index  is  used  in  conjunction  with  the  limits  on  control  gains, 
the  most  important  dynamics  of  the  coupled  system  (equation  3-31)  take  the  form: 


~Mf  +  Af(d) 
BTf(0) 


B^{6)  £/(0)'p/l  rcoof*/ 

J  +  Bm0(9)  BtO(0 )  0f  +  0  Cr  0  9,  + 

Btt0{6)  Br{0)  J|jf  J  LO  0  0  \[g 


K  0  0  o 

o  Kr  o  ef  =  o 

0  0  0  0  T 

r  J  L 


(4-26) 


This  also  assumes  the  elastic  deflections  and  rates  are  relatively  small  compared  to  the 
acceleration  effects. 
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CHAPTER  V 


CONTROL  SCHEME 


5.1  Introduction 

This  chapter  discusses  the  control  scheme  that  will  be  used  to  provide  position  and 
base  vibration  control  of  a  flexible  base  manipulator.  The  goal  of  the  control  scheme  is 
to  add  to  an  existing  position  controller  vibration  damping  capability.  The  rigid  robot 
will  be  commanded  to  provide  the  interaction  forces  and  torques  to  damp  the  vibration. 
An  overview  of  the  control  scheme  is  shown  in  Figure  5-1.  It  is  assumed  the  PID 
position  controller  is  designed  separately  for  rigid  robot  control.  The  rigid  robot  model  is 
given  by  the  third  row  of  equation  3-27.  The  coupled  rigid/flex  dynamics  are  given  by 
the  first  two  rows  of  equation  3-27,  and  the  flexible  manipulator  is  modeled  by  equation 
3-19.  The  reason  this  form  was  chosen  for  the  controller  is  discussed  in  Section  5.4.3. 


Figure  5-1 

Combined  Position  and  Base  Vibration  Control  Scheme 
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First,  the  inverse  kinematics/damping  performance  index  check  really  consists  of  two 
parts.  The  complete  performance  index  will  predict  the  overall  ability  of  the  control 
scheme  to  be  effective.  This  includes  a  comparison  of  the  micromanipulator  and 
macromanipulator  inertia  properties  as  well  as  weighting  matrices  to  include  other 
effects.  The  rigid  robot  performance  measure  that  was  introduced  in  equations  4-2  and  4- 
3  will  be  used  in  the  weighting  matrix  for  the  rigid  robot  and  can  be  used  to  choose  the 
best  inverse  kinematics  solutions  for  inertial  damping. 

Next,  the  vibration  controller  is  introduced.  One  important  goal  of  this  section  is  to 
establish  a  range  of  vibration  control  feedback  gains  to  ensure  vibration  energy  is 
removed  from  the  system.  This  involves  establishing  an  upper  limit  that  will  limit  the 
joint  amplitudes  such  that  the  interaction  effects  due  to  the  joint  accelerations  are  greater 
than  those  due  to  the  joint  velocities,  hence  limiting  the  significance  of  the  nonlinear 
effects.  In  addition,  a  lower  limit  is  established  to  ensure  higher  system  modes  will  be 
damped.  Based  on  the  results  from  Chapter  4,  when  the  performance  index  is  used  along 
with  these  control  gains,  the  most  important  dynamics  take  the  form  of  equation  4-26.  In 
order  to  add  damping  to  the  system,  the  joint  accelerations  will  be  commanded  out  of 
phase  with  the  flexible  base  velocity. 

The  control  scheme  takes  advantage  of  the  fact  that  the  base  vibrations  are  of 
relatively  high  frequency  compared  to  the  rigid  robot  motion  required  to  perform  a  task. 
The  separation  of  bandwidths,  or  time  constants,  between  the  position  and  vibration 
control  loops  allows  them  to  be  considered  separately.  This  is  not  addressed  further  here, 
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but  more  detail  can  be  found  in  references  [11],  [33],  and  [35].  However,  it  is  important 
to  check  the  validity  of  this  assumption  for  the  specific  application. 

It  may  be  desirable  in  some  cases  to  provide  feedback  proportional  to  the  velocity  and 
position,  akin  to  an  ideal  vibration  absorber.  This  can  give  more  flexibility  in  improving 
system  damping  but  requires  additional  measurements  or  manipulation  of  the  measured 
vibration  data.  However,  the  general  form  of  the  controller  as  well  as  guidelines  for 
choosing  feedback  gains  will  remain  the  same  for  either  form. 

Finally,  the  control  performance  of  the  linearized  system  will  be  discussed  in  the 
single  degree  of  freedom  case.  Root  locus  plots  based  on  the  linearized  models  verify 
that,  over  a  range  of  feedback  gains,  damping  can  be  added  to  the  flexible  system.  For 
these  studies,  it  has  been  assumed  the  rigid  robot  model  takes  the  form  of  the  third  row  in 
equation  3-27.  The  hardware  on  which  this  control  scheme  was  implemented  was  a 
hydraulically  operated  robot.  The  actuator  dynamics  dominated  the  performance  of  the 
experimental  robot,  so  this  effect  is  also  discussed  in  Section  5.4.5. 
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5.2  Performance  Index 


The  performance  index  may  be  used  for  two  tasks.  First,  it  may  be  used  to  predict  the 

expected  effectiveness  of  the  inertial  damping  control  scheme.  This  needs  to  include  a 

check  for  inertial  singularity  points  and  include  other  effects,  such  as  the 

macromanipulator  inertia  and  limits  on  allowable  joint  motion. 

The  following  performance  index  will  provide  this  measure: 

PI  =  [xiM(d)TWfM(e)xJ-'[rmBWTWrB(0)0m]  (5-1) 

where: 

xm  =  [maximum  flexible  base  accelerations] 

6  m=  [maximum  rigid  robot  joint  accelerations]  (5-2) 

By  including  the  maximum  base  accelerations  and  joint  accelerations,  a  direct  measure  of 
the  micromanipulator  to  macromanipulator  interaction  forces  and  torques  can  be  made. 
In  addition,  different  limits  on  joint  accelerations  can  be  accounted  for  via  the  maximum 
acceleration  vectors.  This  could  be  important  if  the  actuators  of  the  rigid  robot  have 
bandwidth  limitations  or  can  saturate,  which  may  impact  the  effectiveness  of  this 
technique.  On  the  other  hand,  if  the  rigid  robot  can  accelerate  its  links  rapidy  it  will  be 
more  effective. 

The  micromanipulator  and  macromanipulator  inertia  properties  are  given  by: 

koW 

Mf+Af(6)  Bwf(6)  1 

Aoih  J  +  BmO(0)  J 

where  these  are  the  inertia  effects  from  equation  4-26  linearized  about  an  operating  point. 
It  is  assumed  the  macromanipulator  is  in  a  fixed  joint  configuration  so  the 
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macromanipulator  properties  are  assumed  approximately  constant,  but  in  reality  they  will 
vary  with  the  configuration  of  the  macromanipulator.  Regardless,  this  performance 
measure  will  predict  the  expected  performance  based  on  the  inertia  properties  of  the 
macromanipulator  in  any  configuration.  The  inertia  matrices  times  the  acceleration 
vectors  provide  the  forces  and  torques  due  to  the  flexible  and  rigid  manipulators  in  each 
direction.  In  the  most  general  case,  this  results  in  two  6x1  vectors.  The  transpose  of 
these  vectors  times  the  vectors  of  the  forces  and  torques  results  in  a  constant  equal  to  the 
sum  of  the  square  of  the  forces  and  torques.  However,  this  measure  alone  does  not 
indicate  whether  the  rigid  robot  has  the  ability  to  generate  these  interactions  in  multi¬ 
degrees  of  freedom.  Thus  the  weighting  matrices  need  to  be  included. 

The  rigid  robot  weighting  matrix  is  given  by: 

B(6)T  B  (6) 

w  ~ - N  (5-4) 

This  was  partially  discussed  in  Chapter  4  along  with  its  usefulness  in  determining  joint 
space  configurations  better  suited  for  inertial  damping.  The  only  difference  here  is  that  it 
is  normalized,  essentially  assigning  a  penalty  as  the  rigid  robot  moves  away  from  its  best 
configuration.  Another  advantage  of  normalizing  the  performance  measure  is  that  the 
units  of  the  resulting  BTB  matrix  do  not  matter. 

Some  authors  have  questioned  the  validity  of  eigenvalues  and  singular  values  for 
systems  with  physical  inconsistencies,  such  as  robots  with  mixed  prismatic  and  revolute 
joints  [20,60].  For  example,  the  third  link  of  the  spherical  robot  is  prismatic,  so  the  Bf 
matrix  in  equation  4-6  has  units  of  mass  for  the  third  column  while  the  other  columns 
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have  units  of  mass  times  distance.  This  is  not  a  problem  if  the  determinant  if  Bf  is  taken, 
but  using  the  determinant  introduces  many  other  disadvantages  that  were  discussed  in 
section  4.2.1.  However,  the  weighting  matrix  is  only  intended  to  assign  a  penalty  as  the 
rigid  robot  moves  nearer  singularity  regions.  The  units  of  the  resulting  system  are  not 
important  since  they  are  normalized,  so  the  resulting  weighting  matrix  is  unitless.  N  is  a 
6x6  diagonal  weighting  matrix  which  is  similar  to  the  weighting  functions  used  in 
optimal  control  [23].  Its  purpose  here  is  to  scale  the  torques  by  a  constant  distance  so  the 
total  forces  and  torques  will  have  consistent  units  and  may  be  added,  although  other 
considerations  could  be  included  if  desired.  The  resulting  performance  measure  for  the 
rigid  robot  is  given  by: 


PIr  =  Wr*(  X 

i=x,y,z 


F2 


T2 

i,max  \ 

N  ] 


(5-5) 


where  Fj,max  and  ^i,max  are  the  maximum  interaction  forces  and  torques  that  can  be  created 
by  the  rigid  robot  and  0CWTC1.  The  nearer  the  robot  is  to  a  singularity  configuration, 


the  smaller  Wr  will  be. 

The  flexible  base  weighting  matrix  provides  a  scaling  factor  based  on  the  inverse  of 
the  flexible  base  stiffnesses.  It  is  also  normalized  based  on  the  minimum  stiffness  in  the 
system.  This  has  the  effect  of  reducing  the  weighting  in  stiffer  directions  or  with  higher 
frequency  vibration.  Assuming  harmonic  base  vibration,  the  higher  frequencies  result  in 
lower  amplitude  vibration  and  will  damp  more  quickly  than  the  lower  frequencies.  N  is 
again  a  scaling  factor  to  normalize  the  torques.  The  flexible  weighting  matrix  is  given 
by: 
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0  0  —  0  0  0 
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0  0  0  0 


A*G„ 


0 


0  0  0  0  0 


1 


N*G . 


(5-6) 


The  resulting  perfomance  measure  for  the  flexible  base  is  given  by: 


PI,  =  I  K 


K 


-  + 


Z  Tln 


i-x,y,z 


,max  /  j  ;,max 


Gmin 


(5-7) 


where  Fi!max  and  Ti>max  are  the  maximum  interaction  forces  and  torques  created  at  the  base 
of  the  rigid  robot  due  to  the  macromanipulator  tip  acceleration.  The  weighting  matrix  has 
the  effect  of  reducing  the  magnitude  of  the  interaction  forces  and  torques  due  to  the 
macromanipulator  in  stiff  directions. 

The  overall  performance  measure  compares  the  ratio  of  the  performance  measures 
(PIr/PIf).  Notice  it  will  be  larger  if  the  macromanipulator  has  stiff  directions  that  are  of 
less  concern  for  vibration.  Also  note  it  is  immaterial  whether  the  units  are  English  or  SI 
because  the  weighting  matrices  are  normalized.  The  only  requirement  on  units  is  that 
units  used  for  the  macromanipulator  be  consistent  with  those  used  for  the 
micromanipulator.  Finally,  although  it  is  not  specifically  addressed  in  this  dissertation, 
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the  measure  may  be  extended  to  address  cases  of  underactuation  or  redundancy,  since 
there  is  no  requirement  for  the  B  or  M  matrices  to  be  square. 

The  measure  discussed  above  will  predict,  in  general,  the  ability  of  the  inertial 
damping  scheme  to  successfully  damp  vibration  in  the  macromanipulator.  It  may  be  used 
in  the  control  scheme,  and  for  thoroughness,  the  full  performance  measure  should  be  used 
if  computational  capability  allows  it.  However,  it  may  also  create  an  unnecessary  burden 
on  the  controller  to  carry  out  the  calculations  real-time.  The  reduced  rigid  robot 
performance  measure  introduced  in  equations  4-2  and  4-3  may  provide  a  quicker  and 
more  easily  implementable  measure  to  use  actively  in  the  control  scheme  or  in  the 
development  of  pre-programmed  trajectories.  This  reduced  measure  was  used  in  Matlab 
simulations  and  experimental  work  discussed  in  Chapters  6  and  7  in  order  to  reduce 
computation  time  and  demonstrated  its  effectiveness. 

5.3  Vibration  Controller 

In  most  workspace  locations,  the  inertia  effects  dominate  the  controllable  interaction 
forces  and  torques.  The  performance  index  discussed  above  may  be  used  to  choose 
workspace  configurations  where  the  inertia  effects  are  large.  In  configurations  where  the 
nonlinear  effects  become  large,  the  amplitude  of  joint  motion  can  be  limited  to  improve 
the  ratio  of  inertia  to  nonlinear  effects.  When  this  is  the  case,  the  vibration  controller  will 
prescribe  the  joint  accelerations  out  of  phase  with  the  base  velocity  as  follows: 

0  =  -ID  (6  ,d)Kx  (5-8) 

ID  is  an  inverse  dynamics  function  designed  to  cancel  the  significant  rigid  robot 
dynamics  [4,19]  and  x  refers  to  the  flexible  base  vibration  (Xf).  K  is  a  diagonal  matrix  of 
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gains  Kj,  where  Kj  is  the  gain  for  the  ith  vibrational  degree  of  freedom.  With  the  limits  on 
gains  that  will  be  developed  in  the  next  section,  the  inertia  effects  are  expected  to  be  most 
significant.  Thus  the  prescribed  joint  accelerations  will  be: 

6=-B-'{$)Kx  (5-9) 


However,  the  joint  torques  will  need  to  be  commanded  so  the  final  vibration  controller 
takes  the  form: 


r  =  -BT(e)B-'(6)(Kx) 
Bx(0)  is  as  defined  in  equation  3-27  and 


B  = 


«*(•). 


(5-10) 


(5-11) 


It  is  assumed  the  rigid  robot  joint  positions,  0,  are  measured  and  available  for  use  in 
the  control  scheme.  There  should  be  a  minimum  value  established  for  the  determinant  of 
the  inertia  matrix  to  prevent  it  from  being  inverted  when  the  robot  is  passing  through  a 
singularity  configuration.  Although  these  regions  will  be  avoided  for  point-to-point 
motion  and  fixed  configuration  operation,  it  may  be  necessary  to  pass  through  the 
singularity  regions.  In  this  case,  it  will  become  necessary  to  limit  the  commanded  output 
from  the  controller. 


5.3.1  Vibration  Control  Gains 

The  goal  of  this  section  is  to  establish  a  range  of  vibration  control  feedback  gains  to 
ensure  vibration  energy  is  removed  from  the  system.  The  upper  limit  is  developed  in 
order  to  limit  the  joint  amplitudes  such  that  the  interaction  effects  due  to  joint 
accelerations  (inertia  effects)  are  greater  than  those  due  to  the  joint  velocities,  hence 


103 


limiting  the  significance  of  the  nonlinear  effects.  In  addition,  a  lower  limit  is  established 
to  ensure  higher  system  modes  will  be  damped. 


5 .3. 1.1  Upper  Limit 

In  order  to  establish  an  upper  limit  on  the  feedback  gains,  first  assume  harmonic  base 
vibration  of  mode  i.  As  discussed  in  Chapter  4,  assuming  harmonic  motion  of  the  rigid 
robot,  the  inertia  effects  will  be  functions  of  the  joint  accelerations  and,  with  the  proper 
limit  on  the  amplitude  of  joint  motion,  will  be  greater  than  the  nonlinear  effects.  Thus, 
the  vibration  controller  will  prescribe  the  joint  accelerations  out  of  phase  with  the  base 
velocity  in  order  to  add  damping  to  the  flexible  base. 

The  prescribed  joint  accelerations,  velocities,  and  positions  for  the  jth  joint  will  be 
approximately  harmonic  and  take  the  form: 


xi  =  X,  sin  (Ojt 
6  j  =-B~'(d)KiXio}icoswit 


ej=-B-l(6)KlXlsma>it 


=  B^)KJX1 

1  CO-. 


COS&ft 


(5-12) 


Here  is  it  assumed  the  inertia  matrix,  B(0),  can  be  linearized  and  is  approximately 
constant  about  an  operating  point.  The  feedback  gains  will  be  selected  to  ensure  this  is  a 
reasonable  assumption  and  will  be  discussed  in  more  detail  later. 

The  maximum  amplitude  of  the  prescribed  joint  motion  will  occur  during  the  first  few 
cycles  of  vibration  damping  and  for  each  joint  can  be  written  as: 


W.EgiBL.A 


CO- 


(5-13) 
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where  A  is  defined  as  the  amplitude  of  motion.  It  is  clear  that  an  upper  limit  on  the 
feedback  gains  is  necessary,  if  for  no  other  reason  than  to  ensure  the  joint  motion  remains 
inside  the  allowable  workspace  or  to  prevent  actuator  saturation.  Another  consideration 
is  the  ratio  of  the  inertia  effects  (functions  of  the  joint  accelerations)  to  the  nonlinear 
effects  (functions  of  products  of  the  joint  velocities).  This  was  discussed  extensively  in 
section  4.3.1,  and  Figure  4-23  showed  that  the  ratio  of  inertia  effects  to  nonlinear  effects 
can  be  improved  by  reducing  the  amplitude  of  joint  motion.  Here,  the  centrifugal  effects 
are  considered,  so  these  become  functions  of  the  square  of  the  joint  velocities: 


_coiB(6) 
-B-\d)K?X?  KiXi 


Solving  equation  5-13  for  Kj  and  substituting  into  equation  5-14  yields: 


Aaf  _  1 
A2a>f  A 


(5-14) 


(5-15) 


Likewise,  the  ratio  of  inertia  to  coriolis  effects  takes  the  form: 


e. 


CO; 


eiej  AjCOj 


(5-16) 


where  ©j  and  ©j  are  two  different  frequencies  of  base  vibration. 

Notice  by  relating  the  feedback  gains  to  the  amplitude  of  joint  motion,  the  ratios 
become  inversely  proportional  to  the  amplitude  of  motion,  A.  Thus,  A  can  be  limited  to 
ensure  the  joint  acceleration  effects  remain  larger  than  joint  velocity  effects.  One  upper 
limit  is  A<1  radian,  although  there  may  be  more  restrictive  limits  due  to  other 
considerations.  The  gains  should  be  limited  such  that: 
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(5-17) 


'  X 

■<a<  max 

The  true  multi  degree-of  freedom  case  is  more  complex,  of  course.  In  addition,  the 
selection  of  A  and  Bmjn  will  be  specific  to  the  macro/micromanipulator.  The  inertia  and 
force  effects  also  vary  throughout  the  workspace,  as  discussed  in  Chapter  4.  However, 
the  above  limit  will  help  reduce  the  significance  of  the  nonlinear  effects,  even  when  in 
workspace  locations  where  they  can  become  large.  Also,  note  this  ratio  improves  with 
decreasing  amplitude,  which  increases  the  effectiveness  of  the  scheme  as  the  vibration  is 
damped. 

This  limit  also  helps  validate  the  assumption  that  the  inertia  matrix  B  can  be 
linearized  about  an  operating  point.  Consider  the  sensitivity  of  the  inertia  matrix  to 
changes  in  the  joint  positions.  In  order  to  keep  this  argument  general,  these  matrices 
consist  of  terms  involving  sines,  cosines,  and  combinations  of  sines  and  cosines.  This 
general  argument  is  intended  to  provide  an  example  representative  of  all  of  the  rigid  robot 
effects  (inertia,  nonlinear,  and  gravity  matrices).  In  order  to  determine  the  error 
introduced  by  evaluating  B  at  an  operating  point,  expand  B  in  a  Taylor  series  [6]: 


B($)  =  B(S)  +  A6t 


(5-18) 


where  A0j  is  harmonic  and  given  by  the  last  equation  in  5-12.  The  inertia  matrix  B  as 
well  as  its  first  partial  derivatives  are  bounded  and  continuous.  The  limits  on  joint 
amplitudes  will  ensure  the  variation  term  remains  small.  Also,  note  this  assumption 
becomes  more  valid  as  energy  is  removed  from  the  system  because  the  joint  amplitudes 
are  reduced.  This,  in  general,  is  applicable  to  all  of  the  rigid  robot  effects. 
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5.3. 1.2  Lower  Limit 


A  lower  limit  also  needs  to  be  established  to  ensure  higher  flexible  system  modes,  if 
excited,  will  be  damped.  The  nonlinear  effects  will  still  be  commanded  along  with  the 
damping  effects.  Here  a  worst-case  scenario  is  considered  where  the  nonlinear  effects 
excite  a  mode  of  the  flexible  system.  The  goal  here  is  ensure  net  energy  removal  from 
the  system. 

Assume  an  initial  impulse  force  6  (N)  applied  over  a  finite  amount  of  time  T  excites  a 
fundamental  mode  of  the  flexible  system.  The  rate  of  change  of  energy  added  to  the 
flexible  base  is: 


dW 

dt 


-8x 


=SXia>i  cos  oaf, 


(  N-m\ 


V  s  J 

where  the  base  motion  is  given  by  the  first  equation  in  5-12. 
flexible  base  is  given  by: 


(5-19) 

The  energy  added  to  the 


AW  =  £  8(0 iXi  cos cojdt 
=  JXjSinu^lg 

=  £XiSin<u,T  (N-m)  (5-20) 


The  rate  of  change  of  energy  dissipated  by  the  damping  controller  is  [59]: 


dW 

dt 


=  -K,x2 


(5-21) 


Over  one  cycle  of  vibration,  the  net  energy  dissipated  by  the  vibration  controller  is: 
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2 £ 
ft): 


2/r 

Aff  =  -KtcofXf  pT cos2  cojdt 


=  -K,a>tXfa 


t 

— + 
2 


sin  2<»(t 
4n)(. 


(5-22) 


This  holds  regardless  of  whether  velocity  feedback  alone  is  used  or  if  velocity  and 
position  feedback  are  used.  This  is  the  energy  removed  over  each  cycle  of  vibration,  so 
as  long  as  the  disturbance  is  applied  over  a  finite  period,  the  vibration  controller  will 
remove  energy  from  the  system. 

Based  on  linearizing  the  system  and  assuming  harmonic  motion  over  each  cycle  of 
vibration,  the  nonlinear  centrifugal  forces  take  the  form  (the  same  form  applies  to  the 
nonlinear  torques): 


Fm 


Ni 


Nf(d)$2j 


Fm  =  Nf(d)B~2(0)K?X?  sin2  mjt 


Fm  = 


N f(B)B~2  (8)K?X? 


(1  -  cos  2 cOjt) 


(5-23) 


where  Nf  represents  Ncf  or  Nc-to  given  in  equation  4-11  for  forces  and  torques, 
respectively  and  (sinoojt)2  was  replaced  by  an  equivalent  trigonometric  identity.  Define  Y 
as  the  amplitude  of  the  nonlinear  forces,  or: 


Y(6)  = 


Nf  (0)B~2  (d)K2Xf 
_ 


(5-24) 


Y  may  be  assumed  approximately  constant  since  Nf  and  B  may  be  assumed 
approximately  constant  about  an  operating  point,  as  discussed  in  section  5. 3. 1.1.  The 
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nonlinear  force  may  be  split  into  two  terms,  Y  and  -Ycos2©it.  The  rate  of  change  of 


energy  added  to  the  system  due  to  the  first  term  is  given  by: 


dW  T,.r. 

- =  Y(Q)Xicoi  cos  cott 

dt 


(5-25) 


Regardless  of  the  mode  or  number  of  modes  excited,  if  Y  is  approximately  constant  the 
net  work  over  each  cycle  of  vibration  will  be  small  since: 

In 

A  Win  =  Y(0)  J X.a>l  cos cajtdt  *  0  (5-26) 

and  the  vibration  controller  will  remove  any  energy  added. 

The  effect  of  the  second  term  is  more  interesting.  Define  the  first  harmonic  of  the 
fundamental  frequency  as: 

(0=2(0,  (5-27) 

Consider  the  flexible  base  as  a  damped  system  responding  to  a  harmonic  forcing,  or 

Mx2  +  Cx. j  +  Kx2  =  -Y(6)  cos  cd't  (5-28) 

where  M,  C,  and  K  are  inertia,  damping,  and  stiffness  properties  of  the 
macromanipulator  and  the  natural  frequency  and  damping  ratio  of  the  flexible  base  mode 
are  given  by  con  and  respectively.  Assume  the  resulting  vibration  will  take  the  form: 

x2  =  X2  cos {(df  -</>)  (5  -29) 

where  X2  is  the  amplitude  of  the  vibration  and  (j)  the  resulting  phase  shift. 
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Substituting  5-29  into  5-28  and  solving  for  the  resulting  amplitude  and  phase  shift  yields: 


*2  = 


-Y 


l  =  —  Vk2-®/2)2  +  (2^.)2 


(5-30) 


tan^  = 


Cco, 


K  -  MW, 


—2 


_ 

2  —2 
cot  -  0)f 


(5-31) 


The  rate  of  change  of  energy  into  the  system  and  net  energy  added  over  one  cycle  of 
vibration  becomes: 


dW 

dt 


=  YX 2coi  cos co(t  *  sin (co.t  -  (j>) 


in  in 

A  Win  =  YX2w.  cos ^  coseT,./  sin  ajdt  -  YX  2ai  sin  <p  [“<  cos2  mjdt 


=  -YX2nsm<, 


(5-32) 


Substituting  5-30  into  5-32  gives  the  net  energy  added  to  the  system  over  one  cycle  of 
vibration: 


&W,„  = 


Y2  sin  <t>7t 


(5-33) 


The  net  energy  dissipated  by  the  vibration  controller  over  one  cycle  of  vibration  is: 


A  TT/  r/  T/-  2  -  ^ [Y  CO  :7C 

AKu,  =  ~KiX2coin  = - 


(5-34) 


In  order  to  ensure  energy  dissipation,  the  goal  is  to  choose  Kj  such  that  AWout+AWjn<0. 
This  relationship  becomes: 


-K,^  +  L  sin  <f>  <  0  (5-3  5) 

The  worst-case  scenario  occurs  when  the  nonlinear  harmonics  occur  exactly  at  a  natural 
frequency  of  the  system,  or  when  co  =  co  and  sin  6  =  1  in  which  case: 

i  n  t 


no 


(5-36) 


M 


Thus  the  lower  limit  on  the  control  gain  to  ensure  net  energy  dissipation  becomes: 

2£», 


K ,  >  ±- 


M 


(5-37) 


Consider  the  meaning  of  this  lower  limit.  As  long  as  the  vibration  control  gains  are 
selected  such  that  the  vibration  controller  adds  no  more  energy  to  higher  system  modes 
than  that  which  can  be  dissipated  by  the  natural  damping  in  the  system,  overall  vibration 
energy  will  be  removed  from  the  system.  However,  this  lower  limit  analysis  only 
considers  the  higher  system  modes.  In  order  to  prevent  excitation  of  them  along  with 
other  negative  consequences  of  using  negative  feedback  gains,  the  lower  limit  is 
realistically  chosen  as  zero.  Using  the  upper  limit  established  previously,  limiting  the 
gains  to  the  range: 

co.  ■  B(6 )  . 

0  <  K,  <  '•min  —  A  (5-38) 

^  i,  max 

will  ensure  effects  due  to  joint  accelerations  are  larger  than  those  due  to  joint  velocities, 
hence  the  inertia  effects  will  also  be  larger.  It  also  ensures  there  is  enough  damping 
available  to  successfully  remove  vibration  energy  from  higher  modes  of  vibration  (if  a 
concern)  if  they  are  excited. 


5.4  Controller  Performance 

This  section  investigates  the  control  performance  of  the  linearized  system,  for  both 
ideal  robot  models  as  well  as  a  hydraulically  operated  robot.  Using  the  linearized  model 
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shown  in  Figure  5-2,  the  controllability  of  the  system  is  discussed.  Next,  the  combined 
system  controller  is  discussed.  The  theoretical  development  in  previous  sections  is  based 
on  assuming  increasing  vibration  control  feedback  gains  results  in  a  direct  increase  in 
flexible  base  damping.  This  section  shows  that  this  is  achievable  over  a  range  of 
feedback  gains  and  under  what  circumstances  the  position  and  vibration  controllers 
interfere.  This  is  only  briefly  discussed  for  the  linearized  system  here.  For  more 
extensive  analysis  on  methods  to  decouple  the  two  controllers,  the  reader  is  referred  to 
Book  and  Lee  [11],  Lee[32,33]  and,  more  recently  Lew  [35-38]. 

In  most  workspace  regions,  the  inertia  effects  dominate  the  interactions,  as  discussed 
in  Chapter  4.  The  performance  index  will  be  used  to  ensure  the  robot  operates  in  these 
joint  workspace  configurations,  whenever  possible.  In  other  regions  where  nonlinear 
effects  can  become  large,  the  joint  amplitudes  will  be  limited  as  discussed  in  section  5.3.1 
to  reduce  the  nonlinear  effects. 
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Figure  5-2 

Linearized  Single  Degree  of  Freedom  Control  Scheme 
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When  these  conditions  are  met,  the  linearized  equations  of  motion  from  equation  3-27 
take  the  form: 

FIF  =  Bf(6)d 

TIF  =  BrO(0)6 

t  =  (5-39) 

The  rigid  robot  control  model  does  not  include  actuator  dynamics,  which  would  be  a 
factor  in  most  real  robotics  systems  and  is  discussed  in  the  next  section.  In  order  to 
shorten  the  notation  in  this  section,  Fif  will  refer  to  both  interaction  forces  and  torques. 


5.4.1  Controllability 

Controllability  can  be  investigated  for  the  linearized,  single  degree  of  freedom  model. 
The  state  space  model  takes  the  form: 


X 

'-C/M  -KIM 

0 

o' 

X 

~B/(BrM) 

X 

1  0 

0 

0 

X 

0 

9 

— 

0  0 

0 

0 

9 

+ 

vbt 

0 

0  0 

1 

0_ 

9 

0 

(5-40) 


The  controllability  matrix  [55]  has  a  rank  of  four  except  when  B=0  (BT  is  finite).  The 
determinant  of  the  controllability  matrix  is  given  by: 

B2K 2 


det  = 


B4MA 


(5-41) 


Assuming  the  form  of  the  controller  given  in  equation  5-10,  the  system  has  full  state 
controllability  by  the  rigid  robot  joint  torques,  t,  except  when  B=0.  Thus  this  is  a 
sufficient  but  not  necessary  condition  for  controllability  of  the  system  since  the  states 
could  still  be  controllable  by  some  alternate  controller  form. 


113 


For  the  multi-degree  of  freedom  case,  this  corresponds  to  an  inertial  singularity  point 
discussed  in  Chapter  4.  In  these  configurations,  the  rank  of  the  controllability  matrix 
reduces  to  two  and,  assuming  the  use  of  the  controller  in  equation  5-10,  the  rigid  robot 
inertia  matrix  is  not  invertible.  Another  problem  situation  occurs  when  the  ratio  of 
macromanipulator  stiffness  to  inertia  (K/M)  becomes  very  small,  which  indicates  an 
extremely  flexible  base  or  large  macromanipulator.  This  would  also  present  a  problem 
since  there  will  be  no  coupling  between  the  rigid  robot  interactions  and  the  mode  of 
vibration.  Note  the  performance  index  described  in  section  5.2  would  also  predict 
impacted  performance  in  all  of  these  scenarios. 

5.4.2  Ideal  Rigid  Robot  Model  Control  Performance 

The  general  case  of  position  and  velocity  feedback  is  investigated  here  (gain  limits 
developed  in  Section  5.3.1  are  applicable  for  either  case).  Simulations  were  built  using 
velocity  and  position  feedback  as  well  as  the  more  specific  case  of  velocity  feedback 
only.  Experimental  work  used  acceleration  feedback  with  actuators  that  act  as  a  velocity 
source. 

The  compensated  closed  loop  transfer  function  is  affected  by  both  the  joint  controller 
and  vibration  controller  and  has  a  characteristic  equation  given  by: 

PD  Kv(s  +  ~jf-) 

characteristic  equation  =  \  A - -  - - - - — —  (5-42) 

Bvs 2  Ms2  +Cs  +  K  ' 

PDpos  represents  a  proportional-derivative  position  controller  for  the  rigid  robot.  The  last 

term  is  the  vibration  controller.  The  interaction  forces  and  torques,  are  prescribed  as: 
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(5-43) 


Fif  h if  =  ~Kv(x  + 


K, 


K 


■x) 


dv 


and  are  generated  by  accelerating  the  links  of  the  rigid  robot.  The  transfer  function  with 
the  vibration  controller  feedback  gain  Kv  as  the  multiplying  factor  takes  the  form  [55]: 

K„ 


B/Kv(s  +  -f-) 


G(s\ , 


K 


dv 


vibration  control 


(Ms2  +  Cs  +  K )  *  (. Brs 2  +  Kds  +  K ) 


(5-44) 


Consider  the  typical  case  of  a  flexible  base  with  a  low  fundamental  mode  (<2  Hz)  and 
low  damping.  Under  these  circumstances  the  dominant  flexible  system  poles  are  very 
close  to  the  imaginary  axis  and  at  a  value  roughly  equal  to  the  first  natural  frequency  of 
the  system.  Figure  5-3  shows  a  pole-zero  map  of  equation  5-44,  with  lightly  damped 
flexible  poles  near  the  jco  axis,  critically  damped  rigid  position  control  poles,  and  the 
placeable  vibration  control  zero.  To  add  damping  directly  to  the  system,  the  departure 
angle  from  the  complex  pole  (the  top  one  is  considered  here)  would  be  approximately 
225°,  as  shown  in  Figure  5-3: 


1 80°  -  90°  + 180°  -  2  *  <p  +  d  =  225° 


(5-45) 


where  <j)  represents  the  angle  of  the  vector  from  the  rigid  control  poles  to  the  flexible  pole 


and  0  the  angle  of  the  vector  from  the  vibration  control  zero  to  the  flexible  pole. 
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Pole-Zero  map 


Figure  5-3 

Pole-Zero  Map  of  Closed  Loop  System 


For  critically  damped  performance  and  in  order  to  limit  the  zero  to  the  left  half  plane 
(0>O°),  the  lower  limit  for  the  rigid  control  poles  is  (/)  >  22.5° .  Note  as  <|>  increases  the 
speed  of  the  rigid  controller  decreases  since  the  poles  are  moving  closer  to  the  imaginary 
axis.  Thus,  the  tradeoff  is  a  limit  on  position  control  bandwidth  if  optimal  damping  is 
desired,  which  corresponds  to  a  limit  of  roughly  2.5  times  the  fundamental  frequency.  At 
the  lower  limit  (<j)=22.5°),  the  zero  would  need  to  be  placed  at  an  infinite  distance  along 
the  negative  real  axis  to  achieve  the  best  vibration  control  damping,  which  of  course  is 
unrealistic.  However,  it  can  be  concluded  that  position  plus  velocity  feedback  of  base 
vibration  gives  the  best  solution  for  combined  position  and  base  vibration  control,  in  the 
case  of  an  ideal  rigid  robot  model. 
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For  example,  placing  the  poles  at  roughly  twice  the  natural  frequency  of  the  flexible 

mode  gives  the  best  location  for  the  zero  at: 

d  =  -45“  +  2*  tan"1  (1/2)  (5-46) 

or  at  roughly  7  times  the  natural  frequency  of  the  flexible  system.  This  results  in  the  root 

locus  plot  shown  in  Figure  5-4  (again  plotted  for  increasing  vibration  feedback  gains). 

The  resulting  improvement  in  flexible  pole  damping  is  as  desired,  but  the  tradeoff  is 

underdamped  rigid  robot  position  control  performance.  If  the  feedback  gains  are  large 

enough  to  achieve  critically  damped  rigid  poles,  the  flexible  system  damping 

improvement  is  diminished.  This  suggests  a  lower  allowance  for  the  independently 

designed  position  control  loop  because  the  natural  frequency  of  those  poles  increases 

when  combined  with  an  inertial  damping  controller. 

The  effect  of  the  position  control  gains  should  also  be  considered.  In  this  case,  the 

open  loop  transfer  function  is  given  by: 


G(s) 


position  control 


(Kds  +  Kp)(Ms2  +Cs  +  K) 
Brs2  (Ms2 +Cs+K  +  Kv(s  +  ^)) 


(5-47) 


K 


dv 


The  root  locus  plot  for  increasing  position  control  feedback  gains  is  shown  in  Figure  5-5. 
The  best  flexible  damping  performance  is  achieved  very  quickly  and  at  low  gains  for  the 
position  controller. 
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Imag  Axis  po  Imag  Axis 


System  poles  for  increasing  base  feedback 


Figure  5-4 

Locus  for  Increasing  Vibration  Control  Feedback  Gains 


System  poles  for  increasing  Position  Feedback  Control 


Figure  5-5 

Root  Locus  for  Increasing  Position  Control  Feedback  Gains 
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This  can  also  be  seen  by  considering  the  closed  loop  transfer  function  between  the 


interaction  forces  and  desired  joint  position: 


FjAs) 


Bf 

PD  no,— 

pos 


PD 

1  + - ^ 


K,(s  +  ^) 


+  - 


K, 


dv 


by  Msycs+K 


(5-48) 


Low  position  control  gains  also  help  reduce  the  interaction  forces  due  to  commanded 
movement,  and  hence  unwanted  vibration,  induced  by  the  rigid  manipulator.  Note  using 
the  scheme  shown  in  Figure  5-1,  the  vibration  control  performance  will  not  be  affected. 
During  active  damping  when  the  base  vibration  is  significant,  torque  inputs  required  by 
the  position  controller  are  much  smaller  than  those  due  to  the  vibration  controller.  When 
the  base  vibration  subsides,  the  position  controller  will  ensure  the  robot  is  in  its  correct 
position  and  with  low  PD  gains  any  additional  correctional  movements  will  not  induce 
vibration  back  into  the  system.  On  the  other  hand,  very  low  gains  are  not  desirable 
because  it  will  lead  to  poor  position  control  performance.  A  tradeoff  is  to  ensure  the  rigid 
robot  moves  slowly  enough  that  the  interaction  forces  and  torques  generated  are  not 
significant.  The  interactions  can  also  be  reduced  by  combining  the  vibration  control 
scheme  with  trajectories  that  help  minimize  inducing  vibration  (trapezoidal  velocity 
profile  or  command  shaping,  for  example)  [15,61,68,70].  It  should  also  be  noted  here 
that  these  results  are  based  on  the  ideal  rigid  robot  model;  if  nonlinearities  or  actuator 
dynamics  dominate  the  system  response  these  results  are  not  necessarily  applicable.  One 
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representative  case  is  discussed  in  Section  5.4.5  with  a  rigid  robot  dominated  by 
hydraulic  actuators. 

This  indicates  it  is  important  to  maintain  separation  of  the  performance  of  the  two 
control  loops.  The  work  shown  here  is  a  quick  overview  of  the  controller  under  typical 
operating  conditions.  However,  the  position  controller  can  interfere  with  the  vibration 
controller,  as  shown  in  Figure  5-6.  In  this  case,  the  flexible  system  poles  are  drawn 
towards  the  real  axis,  as  desired.  However,  the  position  control  poles  are  drawn  towards 
zero,  which  results  in  poor  rigid  robot  control  performance  with  increasing  vibration 
control  feedback  gains. 

When  the  critically  damped  rigid  position  control  poles  are  chosen  at  the  natural 
frequency  of  the  flexible  system  and  the  zero  chosen  to  cancel  it,  good  performance 
results  for  both  position  and  vibration  control,  as  can  be  seen  in  Figure  5-7.  With  a  given 
set  of  PD  position  control  gains,  the  vibration  control  feedback  gains  can  be  selected  to 
achieve  the  desired  effect  of  increased  damping  of  the  lightly  damped  flexible  poles 
while  minimizing  impact  on  the  position  control  poles.  In  reality,  perfect  cancellation 
would  not  occur,  but  the  impact  is  minimal  and  in  the  form  of  slightly  reduced  damping 
improvement  or  slight  underdamped  position  control  performance,  as  shown  in  Figures  5- 
8  and  5-9. 
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Example  of  interference  between  position  and  vibration  control 


Figure  5-6 

Root  Locus  with  Poor  Selection  of  Vibration  and  Rigid  Position  Controllers 


System  poles  for  increasing  base  feedback 


Figure  5-7 

Root  Locus  with  Better  Selection  of  Vibration  and  Rigid  Position  Controllers 


When  velocity  feedback  alone  is  used,  improved  damping  is  also  achievable  over  a 
large  range  of  gains  K,  as  can  be  seen  in  Figure  5-10.  For  the  flexible  manipulator  used 
to  develop  these  plots,  the  natural  frequency  of  the  lightly  damped  poles  is  approximately 
2  Hz.  The  maximum  gain  recommended  by  equation  5-38  (assumes  Bmin-A=l)  is 
approximately  K-12,  while  the  maximum  damping  occurs  at  approximately  K=440.  The 
limits  on  gains  will  further  help  ensure  the  vibration  controller  remains  in  regions  where 
increasing  the  gains  directly  increases  damping. 


Velocity  Feedback,  Ideal  Rigid  Robot 


Figure  5-10 

Root  Locus  for  Velocity  Feedback,  Ideal  Model 
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5.4.3  Alternate  Contoller  Form 


One  alternative  to  the  controller  shown  in  Figure  5-2  would  be  one  that  prescribes  the 
vibration  control  via  the  desired  joint  positions,  as  shown  in  Figure  5-11. 


Disturbance 


Figure  5-11 

Alternate  Form  of  Vibration  Controller 

Consider  the  poles  of  the  closed  loop  system  with  the  vibration  control  feedback 
gains  as  the  multiplying  factor  (here  velocity  feedback  is  considered): 


G(s)vil 


Kv(Kds  +  Kp)BTs 3 


vibration  control 


C Ms 2  +Cs  +  K)(Bts 2  +  Kds  +  K) 


(5-49) 


The  pole  zero  map  of  equation  5-49  is  shown  in  Figure  5-12.  Regardless  of  where  the  PD 
position  control  gains  are  chosen,  one  of  the  poles  will  tend  to  move  toward  the  three 
zeros  at  the  origin.  This  tends  to  interfere  with  the  vibration  damping  of  the  lightly 
damped  flexible  poles  and  hence  it  becomes  much  more  difficult  to  design  the  rigid 
position  and  vibration  controllers  independently.  This  increased  interference  between  the 
controllers  can  also  been  seen  by  considering  the  poles  of  the  closed  loop  system  with  the 
position  control  gains  as  the  multiplying  factor: 
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Pole-zero  map 


Figure  5-12 

Pole-Zero  Map  of  Alternate  Controller 


G(S)DOsU« 


position  control 


Kp&s  + 1  )(Ms2  +Cs  +  K  +  KVB/) 

Kp _ 

B/  (Ms2  +CS  +  K) 


(5-50) 


The  poles  of  the  closed  loop  system  will  move  toward  the  zero  with  increasing 
position  control  gains.  Assuming  a  lightly  damped  flexible  system,  for  example  with 
M=20,  C=l,  and  K=3000,  in  order  to  keep  the  zero  driven  by  the  vibration  controller 
in  the  left  half  plane  requires  very  small  values  of  KvBt  (.0066).  This  reiterates  the 
increased  interference  between  the  vibration  and  position  controllers. 
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5.4.4  Simulations  of  Single  Degree  of  Freedom  Controller 

The  single  degree  of  freedom  control  law  was  simulated  in  Matlab  Simulink  and 
takes  the  form  shown  in  Figure  5-13: 


Figure  5-13 

Single  Degree  of  Freedom  Matlab  Simulation 


This  model  simulates  a  single  link  rigid  robot  mounted  on  a  flexible  base,  which  here  is 
simulated  as  a  mass/spring/damper,  as  shown  in  Figure  5-14. 


Figure  5-14 

Single  Degree  of  Freedom  Flexible  Base  Manipulator 
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This  simulates  the  last  link  of  the  robot  shown  in  Figure  5-14  moving  about  an  axis  out  of 
the  page.  This  model  allowed  a  quick  evaluation  of  the  control  scheme  and  its  ability  to 
effectively  damp  vibration  and  maintain  position  control  when  a  disturbance  is  applied  to 
the  system.  The  response  to  a  disturbance  is  shown  in  Figures  5-15  and  5-16.  Figure  5- 
15  shows  much  quicker  damping  with  the  inertial  damping  controller  than  without  it. 
Figure  5-16  shows  the  joint  motion  that  was  used  to  damp  the  base  vibration. 


SrmJated  Base  Motion  Due  to  an  ApfJied  Disturbance 


Figure  5-15 

Simulated  Base  Motion  Due  to  an  Applied  Disturbance 


The  scheme  also  allows  the  robot  to  move  from  one  position  to  another  and  reduce 
induced  vibration  significantly.  Figure  5-17  shows  the  joint  actuating  about  its  desired 
position  in  order  to  damp  the  vibration.  The  resulting  base  vibration  is  diminished  with 
the  use  of  the  vibration  controller,  as  shown  in  Figure  5-18. 
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Theta  (rad)  Theta  (rads) 


Simulated  Joint  Motion  Due  to  an  Applied  Disturbance 


Figure  5-16 

Simulated  Joint  Motion  with  Inertial  Damping  Due  to  an  Applied  Disturbance 


Simulated  Joint  Motion 


Figure  5-17 

Simulated  Commanded  Joint  Motion 
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Simulated  Base  motion  due  to  commanded  joint  movement 


-With  inertial  damping 
-  No  vibration  control 


Figure  5-18 

Simulated  Base  Vibration  Due  to  Commanded  Joint  Motion 


Base  velocity  feedback  alone  can  also  be  used  and  can  be  seen  to  be  effective  at  dampng 
vibration  due  to  an  applied  disturbance,  as  shown  in  Figure  5-19. 


Figure  5-19 

Simulated  Inertial  Damping  Performance  with  Velocity  Feedback 
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5.4.5  Hydraulics  Dominated  Rigid  Robot  Model  Control  Performance 

This  section  extends  the  previous  section  to  consider  the  case  of  a  hydraulically 
dominated  rigid  robot  model.  More  detail  on  the  development  of  the  model  used  may  be 
found  in  Chapter  7.  The  control  loop  takes  the  form  of  Figure  5-20,  again  linearized 
using  the  equations  in  5-39  with  the  exception  of  the  third.  Now  the  rigid  robot  is 
commanded  as  an  approximate  velocity  source.  The  joint  position  controller  is  left  in  a 
general  form  here. 


Figure  5-20 

Actuator  Dominated  Linearized  Rigid  Robot  Model 


Now  the  characteristic  equation  takes  the  form: 

,  PID(s)*Kk  Kms 

characteristic  equation=\+ - — — — H - -  (5-51) 

s(rds  + 1)  (rds  +  l)(Ms2  +Cs  +  K) 

PID  is  the  rigid  robot  proportional-integral-derivative  position  controller.  In  this  case,  if 
Td  is  small,  the  system  can  be  considered  an  appoximate  velocity  source.  The  vibration 
controller  and  resulting  interactions  take  the  form  shown  in  Figure  5-21  (not  considering 
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the  position  controller  for  the  moment).  The  rigid  robot  dynamics  are  shown  on  the  top 
line  with  the  resulting  flexible  system  dynamics  on  the  lower  line. 


Figure  5-21 

Form  of  Vibration  Controller  for  Hydraulics  Dominated  Robot 


The  open  loop  transfer  function  with  the  vibration  control  gain  as  the  multiplying  factor 
is  given  by: 


G(s) 


vibration  control 


(Ms2  +  Cs  +  K)[s(rds  + 1)  +  KhPID(s)] 


(5-52) 


With  increasing  vibration  position  feedback  gain,  the  closed  loop  poles  vary  as  shown  in 
Figure  5-22.  Note  increasing  feedback  gains  Kpv  leads  to  an  increase  in  damping  of  the 
flexible  poles  over  a  relatively  large  range  of  gains.  Also  note  the  slight  increase  in 
natural  frequency  of  those  poles.  This  type  of  rigid  robot  can  provide  an  advantage 
because  often  position  measurements  are  more  readily  available  than  velocity 
measurements  and  can  avoid  the  need  for  numerical  manipulation  of  the  measured 
vibration  data. 
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System  Poles  for  Increasing  Feedback  Gain,  Hydraulics  Model 


Figure  5-22 

Root  Locus  for  Increasing  Vibration  Control  Gains,  Hydraulics  Model 


The  other  advantage  here  is  that,  in  this  case,  it  is  beneficial  to  have  stiffer  position 
control  gains.  In  this  case,  the  rigid  robot  position  and  vibration  controllers  can  be 
designed  more  independently  than  in  the  ideal  model  case.  This  can  also  be  seen  by 
considering  the  poles  of  the  characteristic  equation  in  5-51  with  the  position  control  gains 
as  the  multiplying  factor.  The  root  locus  plot  is  shown  in  Figure  5-23.  It  can  be  seen 
that  increasing  position  control  gains  has  little  effect  on  the  lightly  damped  flexible 
system  poles. 


PID*Kh(Ms2+Cs  +  K ) 

W  position  control  -  (My2  +  ^  +  +  ^  +  R ^2 


(5-53) 
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System  Poles  for  Increasing  Position  Feedback 


Figure  5-23 

Root  Locus  for  Increasing  Position  Control  Gains,  Hydraulics  Model 


Simulations  demonstrating  the  controller  performance  in  multi-degree  of  freedom 
operation  will  be  discussed  in  Chapter  6. 
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CHAPTER  VI 


SIMULATIONS 

6.1  Introduction 

Simulations  were  built  in  Matlab  to  test  the  ability  of  the  control  scheme  to  damp 
vibration  in  multi-degrees  of  freedom.  A  macromanipulator  was  modeled  assuming  three 
degrees  of  freedom  of  vibration  and  using  a  recursive  Lagrangian  technique  as  described 
in  Section  3.2.  Two  modes  of  transverse  vibration  were  assumed  in  each  planar 
direction.  In  order  to  add  a  sense  of  realism  to  the  simulation,  the  third  mode  in  the  z 
direction  was  chosen  based  on  assumed  modes  for  torsional  vibration.  This  was  chosen 
since  torsional  vibration  is  common  for  flexible  links  and,  although  it  is  not  a  concern  for 
single  link  macromanipulators,  it  will  be  a  concern  for  multi-link  manipulators.  In 
addition,  since  torsional  vibration  is  at  a  higher  frequency  than  transverse  vibration,  it 
allowed  an  early  assessment  of  the  ability  of  the  scheme  to  operate  over  a  wide  range  of 
frequencies.  The  resulting  equations  of  motion  for  the  flexible  base  take  the  form  of 
equation  3-19  with  constant  matrices.  A  three  degree  of  freedom  rigid  robot  was  used  for 
damping  and  takes  the  form  of  the  last  equation  in  3-27,  while  the  interactions  are  given 
by  equation  4-1.  The  purpose  of  these  simulations  was  to  test  the  controller  and  gain 
limits  discussed  in  Chapter  5.  In  addition,  the  usefulness  of  the  performance  index 
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described  in  Section  5.2  was  tested  to  determine  joint  configurations  better  suited  for 
inertial  damping. 

The  simulations  were  extended  to  the  case  of  a  two  link  flexible  manipulator  with  a 
six  degree  of  freedom  anthropomorphic/wrist  micromanipulator.  The  flexible  base 
parameters  were  designed  to  give  base  motion  similar  to  the  two  link  experimental 
testbed,  discussed  in  Chapter  7.  The  modes  modeled  in  the  first  link  were  four  transverse 
modes  and  one  torsional  mode,  and  one  transverse  mode  was  modeled  in  the  second  link. 
The  complexity  of  this  model  made  the  simulation  time  extremely  long.  It  was  decided 
the  three  degree  of  freedom  simulation  with  additional  higher  frequency  modes  added 
would  suffice  to  study  multi-degree  of  freedom  vibration  damping  and  sufficiently 
represented  the  testbed.  Thus,  the  baseline  simulations  used  primarily  in  this  study  had 
three  degrees  of  freedom  of  base  vibration  and  used  a  three  degree  of  freedom  rigid  robot 
to  provide  the  vibration  damping. 

Finally,  an  alternate  configuration  of  a  rigid  robot  with  performance  dominated  by 
hydraulic  actuators  was  studied.  As  discussed  in  Chapter  5,  with  a  small  change  to  the 
control  scheme  the  controller  performance  is  nearly  identical  to  that  of  the  ideal  rigid 
robot  performance.  However,  the  effect  of  variation  in  actuator  performance  can  add 
much  more  uncertainty  to  the  system  than  the  equivalent  seen  with  the  ideally  modeled 
robot  with  nonlinearities.  In  particular,  the  effect  of  increasing  the  servovalve  time 
constant  and  higher  order  dynamics  were  investigated  in  simulation,  primarily  prompted 
by  issues  that  manifested  during  laboratory  testing. 
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6.2  Single  Flexible  Link  Macromanipulator  with  Anthropomorphic  Rigid  Robot 
The  overall  schematic  of  the  three  degree  of  freedom  simulation  is  shown  in  Figure  6- 
1.  Starting  at  the  left  of  the  model,  trajectory  inputs  in  the  form  of  desired  joint 
trajectories  are  the  inputs  to  the  rigid  robot.  The  trajectory  inputs  were  either  constant 
joint  positions,  used  to  test  the  response  of  the  vibration  controller  to  a  disturbance  input, 
or  point-to-point  trajectories.  In  order  to  test  the  use  of  the  performance  index  in 
predicting  inertial  damping  performance,  point-to-point  trajectories  were  generated  from 
desired  starting  and  end  points.  For  each  end  point  in  the  workspace,  there  are  multiple 
joint  space  configurations  that  can  be  used  to  reach  it  (provided  the  robot  is  not  operating 
at  a  kinematic  singularity,  which  is  assumed  for  this  work).  In  each  case,  one  of  the  joint 
configurations  gives  predicted  better  inertial  damping  performance  than  the  other  ones. 
Thus,  two  tracks  were  compared  in  these  simulations;  one  is  the  track  given  by  the 
beginning  and  end  points  in  the  better  joint  space  configurations,  and  the  other  is  the 
alternate  joint  space  configuration  track. 

The  rigid  robot  model  was  written  in  the  form  of  an  s- function  [21],  which  gives  more 
flexibility  in  developing  the  system  model  and  allows  modeling  nonlinearities.  The 
inputs  to  the  rigid  robot  are  the  controller  torques,  which  consist  of  the  PED  position 
control  torques  plus  the  additional  contribution  due  to  the  vibration  controller.  The  total 
input  to  each  joint  is  given  by: 

r  =  PID(0d-da)-ID(6a)Kx  (6-1) 

where  PID  is  an  independent  proportional-integral-derivative  controller  (or  some 
variation  such  as  PI  or  PD),  ID  is  the  inverse  dynamics  function  defined  in  Equation  5-8, 
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Figure  6-1 
Matlab  Simulation 


and  K  is  the  vibration  control  feedback  gains.  For  these  simulations,  a  reduced  inverse 
dynamics  function  consisting  of  only  the  interaction  force  effects  was  used,  or 

m)  =  Bt(«)B}\e)  (6-2) 

The  output  of  the  rigid  dynamics  s-function  are  the  states  of  the  rigid  robot,  which  are  the 
joint  positions  and  velocities.  The  joint  positions  are  fed  back  to  the  PID  controller.  The 
joint  velocities  are  differentiated  to  yield  the  joint  accelerations  and  used  to  calculate  the 
interaction  forces  and  torques,  given  by  equation  4-1. 

The  flexible  base  is  also  modeled  using  equation  3-19  as  an  s-function,  with  inputs 
the  interactions  due  to  the  rigid  robot.  There  is  also  an  option  to  apply  a  disturbance 
directly  to  the  flexible  base.  Since  the  states  of  the  flexible  base  are  the  generalized 
coordinates,  the  disturbances  applied  in  each  direction  are  applied  to  all  of  the  states 
governing  the  motion  in  that  direction.  For  example,  an  applied  disturbance  in  the  x 
direction  is  applied  to  all  of  the  generalized  coordinates  that  affect  the  motion  in  that 
direction.  The  states  are  the  generalized  coordinates  and  their  rates,  while  the  outputs  are 
the  overall  base  motion  in  the  x,  y  and  z  directions  and  their  derivatives.  The  applied 
distubances  and  interaction  forces  and  torques  are  also  output  and  recorded.  All  of  the 
states  and  variables  are  sent  to  the  workspace  where  they  can  be  stored  and  analyzed 
later. 

The  mass,  damping,  and  stiffness  properties  of  the  macromanipulator  were 
determined  as  described  in  section  3.2  and  are  given  by  the  5x5  matrices  M,  C,  and  K 
from  equation  3-19.  The  fully  coupled  system  mass  matrix  representing  the  inertia 
properties  of  the  coupled  system  is  given  by: 
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(6-3) 


Mt  =  M  +  A,  + 

where  the  coupling  matrices  are  given  by  the  appropriate  equations  in  Appendix  A.  In 
order  to  form  these  equations  in  the  typical  state  space  form,  the  state  matrix  is  defined 
as: 


(6-4) 


where  I  is  the  5x5  identity  matrix  and  Z  is  a  5x5  matrix  of  zeros.  The  system  states  are 
given  by: 


x = [A  A  A  q2  sx  px  p2  qx  q2  sx  f  (6-5) 

where  pi  and  p2  are  generalized  coordinates  representing  two  transverse  modes  in  the  x 
direction,  q1  and  q2  represent  two  transverse  modes  in  the  y  direction,  and  Si  represents  a 
torsional  mode  of  vibration. 

The  input  to  the  system  is  defined  as: 


Input  - 


M"’F  +  M~]T> 

cz 


(6-6) 


where  CZ  is  a  column  of  five  zeros.  D  are  disturbances  that  can  be  directly  applied  to  the 
system  generalized  coordinates  and  F  is  the  5x1  set  of  interaction  forces  and  torques.  For 
most  of  the  baseline  simulations,  only  the  interaction  forces  were  calculated,  but  for  some 
simulations  both  interaction  forces  and  torques  were  included  as  discussed  in  Section 
6.3.2.  In  the  general  case,  F  is  given  by: 


F  —  F  + 

1  1  IFi  ^ 


dz 


(%i) 


z=L 


(6-7) 
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•  fh 

where  i  represents  the  force  or  torque  applied  to  the  i  generalized  coordinate  and  <j> 
represents  the  associated  mode  shape.  The  resulting  base  motion  in  each  direction  is 
given  by: 


x  =  Pi  +  P2 

y  =  qx+q2  (6-8) 

z  =  5, 


6.3  Simulation  Results 


6.3.1  Disturbance  Rejection 

First,  the  performance  of  the  controller  was  tested  in  three  degrees  of  freedom  with 
the  inertia  and  nonlinear  effects  modeled,  i.e.  the  terms  in  each  of  the  three  equations  in 
3-27  that  contain  base  translational  effects  and  rigid  robot  effects.  This  allowed  an 
assessment  of  the  performance  of  the  controller  and  the  gain  limits  discussed  in  section 
5.3.  In  these  simulations,  velocity  feedback  of  the  base  vibration  was  used.  The 
performance  of  the  vibration  controller  in  a  configuration  of  [45o,20°,-70°],  the 
configuration  shown  in  Figure  6-2,  is  shown  in  Figure  6-3.  The  controller  gains  were 
selected  near  the  upper  limit  prescribed  by  equation  5-38  in  order  to  provide  maximum 
damping  performance.  The  associated  joint  motion  may  be  seen  in  Figure  6-4  and  the 
total  end  point  positions  in  the  x,  y,  and  z  directions  are  shown  in  Figure  6-5.  Each  joint 
actuates  about  its  operating  point  to  provide  the  necessary  interactions  to  damp  the  base 
motion.  As  can  be  seen,  the  scheme  requires  relatively  small  joint  motions  to  damp  the 
vibration. 
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Figure  6-2 

Anthropomorphic  Robot  Configuration  [45o,20°,-70°] 
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Figure  6-3 

Simulated  Base  Vibration  Due  to  an  Applied  Disturbance 
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Joint  1  Motion 


Figure  6-4 

Simulated  Joint  Motion  Due  to  an  Applied  Disturbance 
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Figure  6-5 

Simulated  Total  End  Point  Position  Due  to  an  Applied  Disturbance 


If  the  control  gains  are  not  chosen  according  to  equation  5-38,  the  vibration  controller 
will  still  remove  the  overall  energy  from  the  system  but  it  is  much  less  effective.  As  can 
be  seen  in  Figures  6-6  and  6-7,  when  the  control  gains  are  chosen  at  three  times  the 
recommended  limits,  the  joint  amplitudes  are  much  larger  and  the  controller  works  less 
effectively.  This  also  verifies  the  reduction  in  damping  predicted  in  Figure  5-10  when 
the  feedback  gains  are  increased  past  a  certain  point.  These  results  verify  that  there 
should  be  appropriate  limits  placed  on  the  feedback  gains  for  best  vibration  control 
performance.  Note  that  the  overall  energy  is  still  removed  from  the  system,  and 
relatively  quickly  compared  to  the  free  vibration  case  shown  in  Figure  6-3  (note  the 
different  y-axis  scales  between  the  two  figures). 
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Figure  6-6 

Simulated  Base  Vibration  Due  to  an  Applied  Disturbance,  Large  Vibration  Control  Gains 
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Figure  6-7 

Simulated  Joint  Motion  Due  to  an  Applied  Disturbance,  Large  Vibration  Control  Gains 


Next,  the  controller  was  tested  in  a  worst-case  configuration  where  the  nonlinear 
effects  are  expected  to  be  large  compared  to  the  inertia  effects,  as  predicted  by  Figure  4- 
21.  In  a  configuration  of  [45°,20°,-10°],  which  is  the  configuration  shown  in  Figure  4-22, 
the  resulting  base  vibration  can  be  seen  in  Figure  6-8.  The  associated  joint  motion  may 
be  seen  in  Figure  6-9.  Note  the  slight  increased  vibration  in  the  z  direction  (third  plot  in 
Figure  6-8).  This  indicates  that  with  the  vibration  controller  in  place  motion  by  any  of 
the  joints  creates  interactions  in  all  of  the  vibrational  degrees  of  freedom  of  the  base  since 
the  base  model  is  fully  coupled.  Thus,  all  of  the  joints  used  for  inertial  damping  must 
operate  cooperatively  to  damp  the  overall  vibration  in  the  system.  As  expected,  the 
vibration  controller  is  less  effective  but  still  provides  more  damping  than  the  undamped 
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case.  This  does  indicate,  however,  that  the  control  gains  have  to  be  carefully  selected, 
especially  in  these  regions,  in  order  to  ensure  successful  performance. 
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Figure  6-8 

Simulated  Base  Vibration  with  Robot  in  a  Region  with  Large  Nonlinear  Effects 


6.3.2  Interaction  Torque  Effects 

Interaction  torque  effects  were  originally  included  in  simulations  but  later  removed 
due  to  the  time  and  complexity  involved.  In  addition,  implementation  of  the  torque 
effects  in  the  inverse  dynamics  would  be  extremely  difficult  to  implement  on  the 
experimental  system.  The  combined  performance  index  measure  of  equation  4-3,  shown 
in  Figure  4-11,  indicated  the  torque  effects  should  be  relatively  small.  However,  anytime 
the  interaction  forces  are  prescribed,  the  interaction  torques  are  also  commanded.  This 
section  describes  the  effect  of  including  these  effects  versus  the  interaction  forces  only. 
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Joint  1  Motion 


Time  (s) 

Figure  6-9 

Simulated  Joint  Motion  with  Robot  in  a  Region  with  Large  Nonlinear  Effects 

As  an  example,  consider  the  disturbance  rejection  capability  of  the  control  scheme  in 
a  configuration  of  [45°, 45°, -70°].  This  is  similar  to  the  configuration  shown  in  Figure  6- 
2  except  the  second  link  is  slightly  further  down.  Both  interaction  force  and  torque 
effects  (equations  A-5  through  A-7  and  A- 14  through  A- 16)  are  compared  with 
performance  when  only  the  interaction  force  effects  are  considered.  Both  results  may  be 
seen  in  Figure  6-10.  It  can  be  seen  that  the  vibration  control  performance  is  affected  very 
little  by  considering  the  interaction  force  effects  only.  Thus  it  was  considered  a 
reasonable  approximation  in  most  workspace  locations  to  model  the  full  interaction 
forces  only. 

On  the  other  hand,  consider  the  robot  in  a  configuration  of  [-90°,45°,-70°]  (same 
configuration  for  the  last  two  links,  but  now  joint  1  is  rotated  135°).  The  same 
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comparison  is  made  and  shown  in  Figure  6-11.  The  vibration  controller  still  removes 
energy  from  the  system  in  both  cases,  but  the  performance  with  and  without  the 
interaction  torques  is  very  different.  An  area  of  future  research  is  recommended  to 
investigate  in  more  detail  the  interaction  torques  and  determine  appropriate 
simplifications  to  enable  modeling  them  in  a  reasonable  manner  for  simulation  and 
experimental  implementation.  One  possible  reason  for  this  change  in  performance  is 
discussed  in  Section  7.5.3. 
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Figure  6-10 

Simulated  Base  Vibration  with  Interaction  Forces  and  Torques  Modeled, 
Configuration  [45°,45°,-70°] 
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Time  (s) 


Figure  6-11 

Simulated  Base  Vibration  with  Interaction  Forces  and  Torques  Modeled, 
Configuration  [-90°,45°,-70°] 


6.3.3  Multi-Link  Macromanipulator  Simulations 

Simulations  were  extended  to  a  multi-link  case  with  a  macromanipulator  model  that 
consisted  of  twelve  states.  For  the  first  link,  three  transverse  modes  were  modeled  in  the 
x  direction,  one  transverse  y  mode,  and  one  torsional  mode.  One  transverse  mode  was 
modeled  for  the  second  link.  These  were  chosen  to  be  similar  to  the  modes  seen  on  the 
actual  testbed  and  will  be  discussed  in  Chapter  7.  A  six  joint  rigid  micromanipulator  was 
modeled,  but  only  the  first  three  links  were  used  for  active  damping,  again  similar  to  the 
testbed.  Due  to  the  complexity  and  simulation  time  required,  it  was  decided  to  use  the 
single  link  model  with  additional  higher  modes  of  vibration  added. 
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6.3.4  Combined  Position  and  Vibration  Control  Performance 


The  second  goal  was  to  test  the  ability  of  the  performance  index  to  predict  damping 
performance.  In  this  case,  point-to-point  rigid  robot  motion  was  commanded  to  simulate 
the  robot  end  effector  moving  from  point-to-point  in  the  joint  workspace.  The 
performance  index  was  used  to  choose  the  best  inverse  kinematics  track  for  inertial 
damping.  The  preferred  track  (trajectory  1)  along  with  the  alternate  inverse  kinematics 
track  (trajectory  2)  are  shown  in  Table  6-1.  Note  the  other  two  inverse  kinematic 
solutions  yield  identical  results  since  for  the  anthropomorphic  robot  the  damping 
performance  only  varies  with  the  configuration  of  joints  2  and  3.  Also,  note  these 
simulations  involve  commanded  rigid  robot  motion  only,  while  the  macromanipulator 
joints  are  assumed  fixed.  Thus,  the  induced  vibrations  are  due  to  the  motion  of  the  rigid 
robot  only.  In  reality,  vibration  would  also  be  induced  by  macromanipulator  motions, 
which  would  create  larger  amplitude  initial  disturbances  similar  to  those  discussed  in 
section  6.3.1.  However,  this  research  considers  the  flexible  base  to  be  unactuated,  which 
is  a  limited  case  of  the  general  macro/micromanipulator  problem. 

The  resulting  base  vibration  can  be  seen  in  Figures  6-12  through  6-14.  In  each  case, 
the  bottom  plot  shows  the  base  vibration  due  to  point-to-point  motion  of  the  robot 
following  trajectory  2  in  Table  6.1  without  vibration  control.  The  third  plot  shows  the 
same  trajectory  except  with  the  vibration  controller.  The  second  plot  in  each  figure 
shows  no  vibration  control  with  the  robot  following  Trajectory  1,  and  the  top  plot  shows 
the  robot  following  trajectory  1  with  the  vibration  controller. 
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Table  6-1 

Simulated  Point-to-Point  Motion 


End  Point 

.4 

-.2 

-.2 

.4 

.4 

(m) 

.4 

.4 

-.2 

-.2 

.4 

.4 

.4 

.4 

.4 

.4 

Trajectory  1 

45° 

117° 

-135° 

-27° 

45° 

5° 

0° 

2° 

0° 

5° 

60° 

83° 

104° 

83° 

60° 

Trajectory  2 

45° 

117° 

-135° 

-27° 

45° 

65° 

83° 

107° 

83° 

65° 

-60° 

-83° 

-105° 

-83° 

On 

O 

o 

Time  (s) 

0-1 

5-21 

25-41 

45-61 

65-80 

The  associated  joint  motion  of  the  first  link  can  be  seen  in  Figure  6-15  during  the  first 
leg  of  motion  (movement  from  45°  to  117°).  Plots  of  all  of  the  joints  for  all  of  the  cases 
may  be  seen  in  Figure  6-16  (trajectory  1  is  labeled  with  “PI”).  One  clear  tradeoff  is  that 
the  joint  position  is  affected  when  under  inertial  damping  control.  This  is  especially 
pronounced  at  the  beginning  and  end  of  each  leg,  and  is  especially  noticeable  between  5 
and  10  seconds  in  Figure  6-15.  This  is  expected  since  the  joint  accelerations  are  largest 
when  the  robot  starts  and  stops.  The  motion,  however,  is  used  to  quickly  damp  the 
vibration,  as  shown  in  the  top  plot  in  each  of  Figure  6-12  through  6-14.  The  other 
tradeoff  is  the  increased  amplitude  of  vibration  induced  by  moving  into  the  better  joint 
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configurations.  This  is  expected  since  these  regions  allow  more  coupling,  which  also 
allows  the  robot  to  create  larger  disturbances.  However,  these  regions  also  allow  more 
effective  coupling  to  damp  the  vibration  more  quickly,  while  the  robot  in  the  alternate 
track  is  much  less  effective  at  vibration  damping.  The  conclusion  is  that  the  robot 
modeled  by  equation  3-27,  even  with  nonlinear  effects  included,  works  effectively  at 
damping  vibration  throughout  the  workspace  provided  the  robot  is  able  to  operate  in  joint 
space  configurations  better  suited  for  inertial  damping. 
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Figure  6-12 

Simulated  Base  Vibration  x  Due  to  Point-to-Point  Motion 
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Joint  1  Motion  During  Point  to  Point  Motion 


Figure  6-15 

Simulated  Joint  1  Motion  During  Point-to-Point  Motion 


Commanded  Joint  Motion 


Figure  6-16 

Simulated  Joint  Motion  during  Point-to-Point  Motion 
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The  total  end  point  position  in  the  x  and  y  directions  can  be  seen  in  Figures  6-17  and  6-18 
respectively.  The  goal  is  that,  provide  the  rigid  robot  is  controlled  so  0act  converges  to 
0des,  the  vibration  controller  will  damp  the  base  vibration,  as  shown  in  Figures  6-12 
through  6-14.  Thus,  the  total  end  point  position  will  also  be  controlled  and  will  have  less 
vibration  than  the  system  without  the  vibration  controller. 


Total  End  Point  Position  x  During  Point-to-Point  Motion 


Figure  6-17 

Simulated  Total  End  Point  Position  x  During  Point-to-Point  Motion 
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Total  End  Point  Position  y  During  Point-to-Point  Motion 


Figure  6-18 

Simulated  Total  End  Point  Position  y  During  Point-to-Point  Motion 

Another  interesting  phenomenon  occurs  in  multi-degree  of  freedom  damping 
simulations  that  was  not  predicted  by  the  ideal  single  degree  of  freedom  cases  discussed 
in  section  5.4.  Observe  the  base  vibration  shown  in  Figure  6-12  with  the  vibration 
controller  in  place  (top  plot).  In  this  simulation,  the  vibration  control  gains  were 
prescribed  at  the  upper  limit  given  by  Equation  5-38.  Note  the  decrease  in  frequency  of 
vibration  from  approximately  1.5  Hz  (undamped)  to  approximately  .57  Hz  (with  inertial 
damping).  When  the  same  situation  was  simulated  with  vibration  control  gains  reduced 
in  half,  the  damped  frequency  was  slightly  higher  (.61  Hz)  and  the  damping  improvement 
is  slightly  better,  as  shown  in  Figure  6-19. 

Recall  these  simulations  include  nonlinearities  in  both  the  rigid  and  coupled 
dynamics,  the  flexible  base  is  fully  coupled  and  models  multiple  modes  of  vibration,  and 
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the  simulations  are  for  a  three  degree  of  freedom  rigid  robot.  The  root  locus  of  the  ideal, 
linearized  case  analyzed  in  Figure  5-10  and  shown  again  below  in  Figure  6-20,  only 
approximates  the  true  multi-degree  of  freedom  case.  These  results  could  indicate  several 
things.  First,  it  could  indicate  that  the  maximum  damping  available  occurs  at  a  much 
lower  gain  than  predicted  (case  1).  It  could  also  indicate  the  flexible  poles  move  away 
from  the  jo  axis  with  a  steep  slope  (case  2),  which  would  result  in  the  observed  decrease 
in  frequency  of  vibration  with  increasing  vibration  control  gains  (case  2).  Note  Figures 
5-4  and  5-9  predicted  this  behavior  could  occur. 

As  noted  previously,  the  rigid  robot  and  vibration  controllers  need  to  be  designed 
carefully  to  ensure  ideal  performance  predicted  by  Figure  5-10.  When  nonlinearities  and 
other  inaccuracies  are  included,  even  in  simulation,  this  ideal  performance  may  not  occur. 
It  is  likely  achievable  with  the  proper  modification  of  the  rigid  joint  controller  and/or 
lower  vibration  control  gains.  Nevertheless,  the  multi-degree  of  freedom  simulations  do 
indicate  the  damping  controller  can  remove  overall  system  energy  and  improve  vibration 
performance  over  the  system  with  no  vibration  damping. 
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Imag  Axis  W  Base  Position  x  (m) 


Effect  of  Reducing  Feedback  Gain  on  Base  Vibration  x 


Figure  6-19 

ise  Vibration  x  with  Reduced  Gains  for  Damping  Controller 


Velocity  Feedback,  Ideal  Rigid  Robot 


Real  Axis 

Figure  6-20 

Root  Locus  for  Single  Degree  of  Freedom  Ideal  Linearized  Model 


6.4  Hydraulic  Actuator  Effects 


The  hardware  implementation  was  performed  on  a  hydraulically  actuated  robot.  In 
order  to  enhance  the  clarity  of  the  discussion  and  focus  on  the  key  issue  at  hand,  a  single 
degree  of  freedom  case  is  discussed  here.  The  simulation  is  shown  in  Figure  6-21.  The 
actual  robot  is  hydraulically  operated  and  the  joint  performance  was  experimentally 
determined  to  be  modeled  as: 


m  =  ku 

t(s)  s(TdS  + 1) 

which  is  typical  of  hydraulic  actuators  [50,48,75]. 
robot  used  in  laboratory  testing,  see  Chapter  7. 


(6-9) 

For  more  information  on  the  actual 


Figure  6-21 

Hydraulics  Dominated  Single  Degree  of  Freedom  Model 
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First,  the  baseline  damping  performance  for  the  inertially  damped  and  undamped 
cases  is  shown  in  Figure  6-22  along  with  the  joint  motion  in  Figure  6-23.  As  these 
figures  indicate,  provided  the  joint  acts  as  a  velocity  source  the  control  scheme  works 
well.  Note  the  joint  motion  is  nearly  out  of  phase  with  the  vibration  displacement  as 
commanded,  allowing  the  vibration  energy  to  be  absorbed.  Joint  2  was  chosen  for  this 
particular  test  case  because  it  demonstrated  the  largest  amount  of  phase  lag.  Next,  the 
same  situation  was  simulated  except  with  larger  td  (as  defined  in  equation  6-9),  which 
increases  phase  lag.  The  resulting  base  vibration  and  joint  motion  is  shown  in  Figure  6- 
24.  As  shown  in  6-24,  the  phasing  of  the  joint  motion  relative  to  the  vibration  has  shifted 
and  no  longer  effectively  removes  vibrational  energy.  As  can  be  seen,  the  effectiveness 
of  the  scheme  decreases,  but  it  is  still  effective.  Provided  the  actuator  can  truly  be 
modeled  as  a  second  order  system,  the  phase  lag  will  be  no  greater  than  90°  and  the 
vibration  control  scheme  will  still  work,  although  the  more  phase  lag  the  less  effective  it 
will  become. 
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Phasing  of  Joint  Motion  and  Base  Vibration 


Figure  6-24 

Effect  of  Increasing  Phase  Lag  on  Hydraulics  Model  Vibration  Performance 

On  the  other  hand,  if  the  hydraulic  dynamics  are  actually  higher  order,  as  has  been 
proposed  by  some  authors  [50],  larger  phase  shifts  may  result  and  create  problems  using 
this  scheme  for  vibration  control.  As  as  example,  consider  the  hydraulic  actuator  modeled 
as  a  third  order  system.  The  phasing  between  the  joint  motion  and  the  base  vibration  is 
shown  in  Figure  6-25.  In  this  extreme  example  very  large  phase  shifts  can  cause 
detrimental  effects.  The  conclusion  is  that,  when  the  robot  model  is  not  given  by  the 
typical  robot  model  (third  equation  in  3-27),  it  is  important  to  perform  system 
identification  testing  to  get  an  accurate  robot  model.  Provided  the  robot  dynamics  are 
reasonably  well  known,  appropriate  motion  can  be  commanded  and/or  cancelled  in  the 
inverse  dynamics  function. 
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CHAPTER  VII 


EXPERIMENTAL  WORK 

7.1  Introduction 

This  chapter  describes  the  experimental  testbed  and  test  results.  The  testbed 
consisted  of  a  two  flexible  link  macromanipulator  with  a  six  degree  of  freedom 
anthropomorphic/wrist  rigid  robot  mounted  to  its  last  link.  The  experimental  work 
consisted  of  two  primary  areas: 

1)  Experimental  verification  of  the  interaction  forces  and  torques  and  conclusions 
described  in  Chapter  4.  The  main  area  of  interest  was  on  the  controllable  interactions  due 
to  the  rigid  robot. 

2)  Implementation  of  the  vibration  control  scheme  on  a  multi-degree  of  freedom  testbed 

This  chapter  will  end  with  a  discussion  on  issues  that  arose  during  hardware 
implementation  and  recommendations  for  improving  the  existing  testbed. 


7.2  Experimental  Testbed 

A  schematic  overview  of  the  laboratory  setup  is  shown  in  Figure  7-1.  One  of  the 
common  problems  with  experimental  results  performed  on  macro/micromanipulators  is 
they  lack  the  complexity  of  real-world  applications  [10].  For  this  reason,  the  testbed  was 
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intentionally  designed  to  be  as  realistic  as  possible.  The  testbed  was  modified  from  a 
single  link  flexible  beam/three  degree  of  freedom  micromanipulator,  used  for  research  in 
[42],  to  a  two  flexible  link/six  degree  of  freedom  micromanipulator.  The  real-time 
control  is  handled  by  VxWorks  Real  Time  Control  Software,  Version  5.2  [85,86],  All  of 
the  control  functions  that  interface  with  VX  Works  were  written  in  C  [29,43].  The 
control  is  performed  using  a  Motorola  68040  Microprocessor  and  Acromag  AVME  D/A, 
A/D,  and  DIO  boards  for  processing  of  the  various  signals  [44,67]. 

The  rigid  robot  consists  of  a  hydraulically  operated  anthropomorphic  robot  with  an 
IBM  7565  wrist  mounted  to  its  last  link  [27].  The  anthropomorphic  robot  is  operated  by 
Micro-Precision  Textron  SS-.5A  double  vane,  SS-.5A  single  vane,  and  SS-1A  double 
vane  actuators,  while  the  wrist  is  operated  by  similar  vane  actuators.  The  servovalves  are 
Moog  series  30  and  31,  which  operate  at  a  constant  maximum  current  of  15  ma  [51,76]. 
The  outgoing  voltage  signal  from  the  D/A  board  is  first  sent  through  voltage  to  current 
amplifiers  before  the  signal  is  sent  to  the  valves.  US  Digital  optical  encoders  are  used  to 
measure  the  joint  position  of  the  first  three  links  of  the  robot,  while  potentiometers  are 
used  for  the  last  three  joints.  Vibration  is  measured  by  PCB  Accelerometers,  one 
mounted  along  each  axis  (x,  y,  and  z)  [57].  During  the  initial  part  of  the  experimental 
work,  a  six  axis  ATI  Delta  9105-T  Force/Torque  sensor  was  used  to  measure  interaction 
forces  and  torques  at  the  base  of  the  rigid  robot  [1], 
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The  range  of  motion  of  each  joint,  with  coordinates  defined  in  Figure  7-1,  is: 


-90°  <#,  <180° 

0°  <02<  95° 

-110°  <  03  <110° 

-0°  <04<  270° 

-90°  <05  <  90° 

-180°  <06  <180° 

All  angles  are  defined  such  that  0°  is  along  the  x  axis,  except  for  joint  2  which  is  defined 
non-conventionally  to  be  at  0°  when  the  arm  is  up  horizontally  and  90°  when  straight 
down. 
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7.2.1  Rigid  Robot  Independent  Joint  Controllers 


The  dynamics  of  the  experimental  robot  are  dominated  by  the  hydraulic  valves.  The 
experimental  rigid  robot  is  shown  in  Figure  7-2.  Each  joint  was  independently  controlled 
with  a  proportional-integral  (PI)  controller  and  a  dead  zone  inverse  (DZI),  as  shown  in 
Figure  7-3.  The  PI  gains  were  chosen  based  on  the  hydraulic  valve  models,  which  were 
experimentally  determined  by  open  loop  system  identification  testing.  DZI  is  a  dead- 
zone  inverse  that  was  designed  to  compensate  for  hydraulic  actuator  dead  zone.  It  is  not 
the  common  dead  zone  inverse  as  described  in  [74],  but  only  includes  an  additional 
constant  signal  output  to  counter  the  effects  of  the  valve  dead-zone  (zero  output  is  offset). 

The  rigid  robot  torque  equation,  or  last  equation  in  3-27,  is  given  by  the  actuator 
specific  model.  In  the  case  of  Moog  servovalves,  a  first  order  approximation  for  low 
frequency  performance  was  used  and  is  given  by  [75]: 


m  k 

i(s)  Tds+ 1 


(7-1) 


Where: 

Q=flow  rate  (cubic  inches) 
i=current  input  (ma) 

K=servovalve  static  flow  gain  at  zero  load  pressure  drop  (cubic  inches/ma) 
Ta=apparent  servovalve  time  constant  (sec) 


167 


Figure  7-2 

Experimental  Rigid  Robot 


Figure  7-3 

Independent  Joint  Controller 
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However,  the  relationship  between  the  commanded  computer  output,  x,  and  the  resulting 
joint  motion,  0a,  was  needed.  K  generally  depends  upon  the  rated  flow  and  input  current, 
while  id  depends  on  the  flow  capacity  of  the  valve.  Since  the  flow  rate  is  proportional  to 
robot  velocity  and  current  proportional  to  the  D/A  output  to  the  robot,  the  rigid  robot 
model  becomes: 


0(s) 


K„ 


(7-2) 


r(s)  s(rds  +  l) 

Open  loop  tests  were  performed  to  determine  the  appropriate  modeling  constants  for  each 
joint.  One  representative  example  may  be  seen  in  Figure  7-4,  which  shows  the  response 
of  joint  1  to  an  output  signal  of  -100,  which  corresponds  to  an  output  voltage  of 


approximately  -.54  volts.  These  tests  were  performed  over  a  range  of  output  values 
expected  for  inertial  damping  control  and  the  results  averaged  over  the  range  of  values. 
The  resulting  model  parameters  may  be  seen  in  Table  7-1 . 

Table  7-1 


Model  Parameters  for  Second  Order  Approximation  of  Moog  Servovalves 


Kh 

Xd 

Joint  1 

-.003459 

.00379 

Joint  2 

-.00076 

.03445 

Joint  3 

-.001775 

.017704 

Joint  4 

.005158 

.031648 

Joint  5 

.00394 

.00356 

Joint  6 

.006 

.0248 
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Theta  1 


Figure  7-4 

Joint  1  Response  to  D/A  Output  of  -100 


An  additional  concern  with  the  hydraulic  actuators  is  an  apparent  dead  zone  behavior 
at  low  output  values.  In  this  case,  the  phenomenon  manifested  itself  as  a  lack  of  response 
over  a  range  of  outputs.  A  representative  example  may  be  seen  in  Figure  7-5.  This  plot 
clearly  shows  that  when  the  actuators  are  operating  at  a  small  range  of  output  values,  the 
joint  does  not  move.  When  it  operates  outside  of  this  range  the  joint  responds  at  the 
approximate  velocity  predicted  by  Kh.  The  first  three  joints  demonstrated  a  smaller  dead 
zone  than  the  last  three  joints  and  were  the  joints  used  for  active  damping  in  this  research. 
The  dead  zone  has  little  effect  on  the  performance  of  the  damping  controller  in  this  case, 
but  may  become  more  of  an  issue  when  additional  links  are  used  in  future  work.  In 
addition,  the  first  three  joints  of  the  robot  demonstrate  a  constant  null  bias  and  will  drift 
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to  the  upper  or  lower  joint  limit.  Unlike  many  other  Moog  servovalve  models,  these  do 
not  have  a  null  bias  adjustment  so  a  constant  feedforward  signal  was  added  to  the 
controller  to  compensate  for  this.  The  resulting  compensator  takes  the  form  of  equation 
7-3,  with  the  parameters  shown  in  Table  7-2. 

If  r  >  0  t  =  r  +  br+d 

If  r  <  0  t  =  r-b,  +d  (7-3) 


Table  7-2 


Feedforward  Dead  Zone  and  Drift  Compensai 

tor 

br 

b, 

d 

Joint  1 

38 

-38 

-20 

Joint  2 

17 

-17 

12 

Joint  3 

31 

-31 

23 

Joint  4 

70* 

-70* 

0 

Joint  5 

150* 

-150* 

0 

Joint  6 

130* 

-130* 

0 

*  Selected  slightly  lower  than  measured  values  to  prevent  amplification  of  potentiometer  noise 


The  joint  controllers  were  selected  for  reasonably  good  performance  using  a  root 
locus  design  method  and  the  performance  was  experimentally  refined  by  tuning.  The 
resulting  gains  may  be  seen  in  Table  7-3. 
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Joint  1 


Figure  7-5 

Dead  Zone  Phenomenon  in  Hydraulic  Servovalve 


Table  7-3 


Selected  Gains  for  Independent  Joint  Controllers 


Kp 

Ki 

Joint  1 

2000 

200 

Joint  2 

2500 

250 

Joint  3 

1000 

O 

O 

Joint  4 

1200 

720 

Joint  5 

1400 

420 

Joint  6 

1500 

1350 

As  an  example,  consider  the  root  locus  plot  of  the  selected  controller  for  joint  1,  shown  in 
Figure  7-6.  The  zero  at  K;/Kp  was  chosen  near  the  two  poles  at  the  origin  in  order  to 
provide  fast,  overdamped  performance.  Using  this  ratio  of  K,7KP,  the  gains  were 
increased  so  the  robot  operated  smoothly  for  point-to-point  motion.  The  wrist  was 
designed  similarly  except  with  more  integral  gain.  This  was  selected  to  provide  more 
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disturbance  rejection  since  its  observed  lab  performance  was  more  nonlinear  and  it 
demonstrated  a  much  larger  dead  zone  region  than  the  anthropomorphic  robot. 

The  anthropomorphic  robot  was  originally  designed  by  Cannon  [15]  so  mass 
properties  were  available.  Mass  properties  of  the  wrist  were  estimated.  The  inertia 
properties  and  dimensions  of  the  robot  are  given  in  Table  7-4.  The  coordinate  frames  are 
as  defined  in  Figure  7-1,  with  dimensions  defined  per  the  Denavit  and  Hartenberg 
convention  [61]  as  detailed  in  Table  A-l  and  A-3  in  Appendix  A. 

Root  Locus  Design  for  Independent  Joint  PI 


-250  -200  -150  -100  -50  0  50 

Real  Axis 


Figure  7-6 

Root  Locus  Design  for  Independent  Joint  Controllers 
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Table  7-4 


] 

Dimensions  and  Properties  of  Experimental 

Robot 

Linki 

a  i 

di(m) 

mi  (kg) 

rCi(m) 

Ixxi  (kg-m2) 

Iwi  (kg-m ) 

Izzi(kg-m2) 

0 

.3302 

16.0320 

.2413 

.1920 

.1933 

.0936 

i 

0 

.1969 

5.0264 

.1354 

.0299 

.0114 

.0270 

2 

.4001 

0 

5.5799 

.2924 

.0171 

.0799 

3 

.12065 

0 

1.488 

.09721 

.019434 

.0186695 

.007131 

4 

0 

.1 

.05 

.000230 

.000174 

.00007332 

5 

0 

.1 

.05 

.0059179 

.006032 

.0019024 

6 

0 

.155 

.0775 

.0056475 

.004396 

.0017209 

7.2.2  Macromanipulator 

The  macromanipulator  was  assembled  from  two  hollow  T6-6061  aluminum  beams  in  an 
L-shape  and  mounted  vertically  from  an  I-beam  mounted  to  the  ceiling  of  the  laboratory. 
The  mounting  and  attachment  to  the  I-beam  is  shown  in  Figure  7-7.  The 
anthropomorphic  robot  with  wrist  is  mounted  to  the  end  of  the  second  link  as  shown  in 
Figure  7-8. 


174 


Figure  7-7 

Macromanipulator  Base  Mounting 


The  macromanipulator  parameters  are: 
do=.1412875  m  (outside  diameter) 
dj=.  134188  m  (inside  diameter) 
p=2710  kg/m3  (material  density) 
Li=4.6482  m  (length  link  1) 
L2=1.2192  m  (length  link  2) 
E=6.8948xl010  Pa 
mi=9.76  kg 
m2=2.56  kg 
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Figure  7-8 

Overall  Macro/Micromanipulator  Testbed 


The  Lagrangian  approach  described  in  section  3.2  was  used  to  model  the 
macromanipulator,  with  the  assumed  modes  for  transverse  vibration  given  by  equation  3- 
5  and  torsional  modes  given  by  equation  3-9.  The  system  was  modeled  as  described  in 
Chapter  6.  The  inertia  matrix  is  fully  coupled,  while  the  stiffness  matrix  is  diagonally 
dominant  with  coupling  between  modes  in  each  direction. 

Lab  testing  of  the  macromanipulator  allowed  some  simplification  of  the  model.  This 
allowed  scoping  the  problem  to  a  suitable  size  to  allow  simulation  in  a  reasonable 
timeframe.  The  configuration  results  in  dominant  and  highly  coupled  transverse  modes 
and  one  torsional  mode  as  well  as  additional  higher  frequency  system  modes.  As  an 
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example,  consider  the  measured  acceleration  in  the  x  direction  due  to  an  applied 
disturbance  in  the  x  direction,  shown  in  Figure  7-9,  along  with  the  frequency  response  of 
the  data  in  Figure  7-10  [5,7,21]. 


Free  Vibration  x 


Figure  7-9 

Free  Vibration  x  Due  to  an  Applied  Disturbance  x 
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Figure  7-10 

Frequency  Content  of  x  Vibration  Due  to  an  Applied  Disturbance  x 

The  acceleration  data  tends  to  amplify  the  higher  frequency  signals,  while  the  lower 
frequencies  are  more  of  a  concern  for  end  point  positioning  due  to  their  larger  amplitude. 
A  more  representative  measure  of  the  vibration  was  chosen  to  be  the  base  position,  which 
requires  double  integration  of  the  accelerations.  It  has  been  noted  elsewhere  challenges 
associated  with  integration  of  piezoelectric  accelerometer  data  due  to  low  frequency  drift 
[66,84].  A  recommended  solution  is  to  high  pass  filter  the  data  prior  to  integration.  For 
presentation  purposes,  the  raw  acceleration  data  was  filtered  using  a  4th  order  high  pass 
butterworth  filter  with  a  cutoff  frequency  of  .8  Hz  [30,54]  before  double  integration. 
Note  this  would  not  be  an  option  for  real-time  control  since  phase  shift  would  be 
introduced  in  the  fundamental  modes.  Figures  7-11  and  7-12  show  the  base  position  and 
associated  frequency  content. 
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The  testbed  has  a  high  degree  of  coupling  between  modes.  Consider  the  y  vibration 
due  to  an  applied  disturbance  in  the  x  direction,  shown  in  Figure  7-13.  Since  these  two 
fundamental  modes  of  the  15  foot  link  are  very  close  in  frequency  (1  and  1.2  Hz),  this 
sometimes  created  a  beating  phenomenon,  which  further  complicated  the  system 
dynamics.  In  general,  excitation  of  any  of  the  modes  tended  to  excite  one  or  more 
additional  modes  in  the  system.  In  order  to  quantify  the  performance  with  and  without 
vibration  control,  free  vibration  damping  ratios  were  obtained  from  the  log  decrement  of 
the  position  data  when  a  single  mode  was  dominant.  For  higher  system  modes  and  in 
cases  of  multiple  mode  excitation,  the  frequency  response  plots  were  used  to  determine 
the  damping  ratios.  The  direction  of  excitation  and  observed  modes  of  excitation  are 
summarized  in  Table  7-5.  Table  7-6  summarizes  the  frequencies  and  damping  ratios  of 
the  prominent  modes. 


Table  7-5 


Modes  of  Vibration  on  Experimental  Testl 

?ed 

Direction  of 
Applied 
Disturbance 

Excited  Modes 
Position  x 
(Hz) 

Excited  Modes 
Position  y 
(Hz) 

Excited  Modes 
Position  z 
(Hz) 

X 

1,4.2 

1,1.2 

1,1.2 

y 

1,1. 2, 4.2 

1.2, 7, 9.5 

1.2, 7, 9.5 

Z 

1,1.2, 4.2 

1.2, 7, 9.5 

1,1. 2, 7, 9.5 

general 

1,1. 2, 4.2 

1.2, 7, 9.5 

1,1. 2, 7,9.5 
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Time  (s) 


Figure  7-13 

Position  y  Due  to  an  Applied  Disturbance  x 


Table  7-6 

Damping  Ratios  for  Macromanipulator  Free  Vibration  Modes 


Mode 

Frequency 
(Hz)  ' 

Damping 

Ratio 

Link  1 
transverse  x 

1 

.0186 

Link  1 
transverse  y 

1.2 

.0067 

Link  1 
torsion 

4.2 

.0087 

Higher  Mode 

7 

.00513 

Higher  Mode 

9.5 

.00281 
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7.3  Interaction  Forces  and  Torques 

The  first  part  of  the  experimental  work  was  intended  to  verify  the  method  used  to 
predict  the  interaction  forces  and  torques  and  verify  work  discussed  in  Chapter  4.  Recall 
the  controllable  interactions  are  given  by: 

¥if  =Bf(6)9  +  Nf(0,6id.)  +  Cf(0y q  +  JV/e(q,q,M) 

T,F=BT0(6)e  +  NtO(6,6i6J)  +  Ct0m  +  Nt0c(qAM  (7-4) 

The  directly  controllable  interactions  that  were  the  subject  of  Chapter  4  are  given  by 
the  first  two  terms  in  each  equation.  However,  the  actual  interaction  forces  and  torques 
are  governed  by  all  of  the  effects,  including  those  due  to  the  motion  of  the 
macromanipulator.  In  order  to  isolate  the  effects  due  to  the  micromanipulator,  the  base  of 
the  macromanipulator  was  braced  as  shown  in  Figure  7-14.  An  ATI  six-axis  force/torque 
sensor  [1]  was  mounted  between  the  micromanipulator  and  the  base  of  the 
macromanipulator.  Input  signals  were  sent  to  the  rigid  robot  similar  to  the  motion 
expected  during  active  inertial  damping,  where  the  motion  is  harmonic  and  given  by  the 
last  equation  5-12. 


7.3.1  Single  Degree  of  Freedom  Interaction  Forces  and  Torques 

The  first  goal  of  this  work  was  to  determine  if  the  relationship  given  by  equations 
4-4  and  4-8,  governing  the  inertia  interaction  forces  and  torques,  provide  a  reasonable 
approximation  of  the  dominant  interactions.  Masses  and  dimensions  of  the  rigid  robot 
were  estimated  as  given  in  Table  7-4.  In  addition  to  proving  the  validity  of  the 
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relationship  for  the  interactions,  it  also  provided  an  opportunity  to  obtain  mass  property 
updates.  The  first  part  of  this  work  involved  actuating  each  joint  independently,  with 
constant  amplitude  sine  inputs  at  1  and  2  Hz,  approximately  the  lowest  natural 
frequencies  of  interest  on  the  tested.  It  was  expected  that  this  would  create  dominant 
inertia  forces  and  torques  since: 

6  =  20°  cos(2*;r*fr) 

180 

0\  «14  rad  Is2 

I  max 

df  » 5  rad  Is2  (7-5) 

I  max 


An  example  of  the  predicted  and  measured  interaction  forces  and  torques  due  to  joint  one 
can  be  seen  in  Figures  7-15  and  7-16,  respectively,  in  a  configuration  of  [-90°,  45°,  45°]. 
This  configuration  was  chosen  because  it  is  near  the  singularity  region,  as  predicted  in 
Figure  4-5,  but  not  near  enough  that  it  should  affect  the  ability  of  the  robot  to  create 
effective  interactions.  Thus  this  configuration  would  be  representative  of  performance 
throughout  most  of  the  usable  workspace  (performance  would  be  better  than  this  in  most 
locations).  The  nonlinear  force  effects  are  included  in  Figure  7-15  for  magnitude 
comparison  purposes. 
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Base  of 

Macromanipulator 


Micromanipulator 


Figure  7-14 

Braced  Micromanipulator 
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Force  (N) 


Time  (s) 


Figure  7-15 

Predicted  and  Measured  Interaction  Forces  at  the  Base  of  the  Micromanipulator  Due  to 

Joint  1  Harmonic  Motion 


In  order  to  quantify  the  results,  a  goodness  of  fit  parameter  was  chosen  [6]  where: 


Time  (s) 


Figure  7-16 

Predicted  and  Measured  Interaction  Torques  at  the  Base  of  the  Micromanipulator  Due  to 

Joint  1  Harmonic  Motion 

Here  x  represents  the  measured  interaction  forces  or  torques  and  y  represents  the 
predicted  interaction  forces  or  torques  calculated  from  the  inertia  effects  only  and  ox  and 
Gy  are  the  standard  deviations  of  each.  Table  7-7  summarizes  these  test  cases,  the  matrix 
terms  isolated  by  each  test  case,  and  the  resulting  goodness  of  fit.  In  general,  the  fits  are 
very  good  with  a  few  exceptions  involving  actuation  of  the  third  link  only.  The  reason 
for  this  is  because  when  these  tests  were  performed,  link  3  had  little  inertia 
(approximately  32.9  kg  or  7.4  lbs).  In  order  to  create  enough  inertia  to  create  interactions 
detectable  by  the  force/torque  sensor,  a  relatively  large  joint  amplitude  had  to  be  used. 
Nevertheless,  the  resulting  interaction  torques  were  still  only  around  +/-60.8  N-m  (+/-  5 


in-lbs).  Either  larger  amplitudes  would  be  needed  for  joint  3,  or  additional  inertia  would 
need  to  be  added.  The  latter  approach  was  taken  for  this  research. 


Table  7-7 


Single  Degree  of  Freedom  Ini 

teraction  Force  and  Torque  Test  Cases 

Test 

Case 

Joint 

Actuated 

Configuration 

(Degrees) 

Amplitude 

(Degrees) 

Frequency 

(Hz) 

Matrix 

Parameters 

Isolated 

■ 

1 

1 

-90,45,45 

10 

1 

■pMPOM 

1 

.989 

.984 

2 

1 

-90,45,45 

10 

2 

KQRB 

3 

2 

0,90,45 

10 

1 

MESPMi 

mz&m 

4 

2 

0,90,45 

10 

2 

5 

3 

0,90,0 

20 

1 

6 

3 

0,90,0 

20 

2 

IBM 

.973 

.608 

7 

1 

0,45,45 

10 

1 

.977 

.978 

8 

1 

0,45,45 

10 

2 

.985 

.979 

9 

2 

-90,90,45 

10 

1 

10 

2 

-90,90,45 

10 

2 

.966 

.976 

11 

3 

-90,90,0 

20 

1 

.973 

.962 

12 

3 

-90,90,0 

20 

2 

13 

2 

0,45,45 

10 

1 

14 

2 

0,45,45 

10 

2 

ml** 

I 

15 

3 

0,90,75 

20 

1 

BA3) 

Bto(2,3) 

.969 

.974 

16 

3 

0,90,75 

20 

2 

Bf(3,3) 

Bx0(2,3) 

.978 

.727 
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7.3.2  Multi-Degree  of  Freedom  Interaction  Forces  and  Torques 

The  next  part  of  this  effort  involved  extending  this  to  interactions  due  to  multiple 
joints  actuating  simultaneously,  which  was  required  to  implement  inertial  damping  in 
multi-DOF.  Figure  7-17  shows  an  example  of  the  interaction  forces  created  by  joint  1 
actuating  at  1  Hz,  joint  2  at  1.5  Hz,  and  joint  3  at  2  Hz  in  a  configuration  of  [45°,  45°, 
60°].  It  was  predicted  that  the  inertia  effects  would  be  dominant  in  this  configuration  and 
the  effect  of  higher  harmonics  would  be  negligible.  The  associated  frequency  response  is 
shown  in  Figure  7-18.  Some  higher  harmonics  are  noticeable  in  the  interactions  but  with 
increasing  frequency  quickly  become  less  important  compared  with  the  inertia  effects. 


Force  y  due  to  Joints  1, 2,  and  3 


-  Measured  Lab 
-Predicted  Inertia 
-Predicted  Nonlinear 


Figure  7-17 

Predicated  and  Measured  Interaction  Forces  Due  to  Joints  1,2,  and  3  Actuating 

Simultaneously 
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Frequency  Content  of  Force  x  Data 


Figure  7-18 

Frequency  Content  of  Force  y  Data  Due  to  Joints  1,2, and  3  Actuating  Simultaneously 

Figure  4-5  predicted  poor  inertia  performance  near  the  inertial  singularity  region  and 
Figures  4-13  and  4-14  predicted  the  nonlinear  coriolis  effects  would  be  more  of  a  concern 
in  these  regions.  Consider  the  robot  in  a  configuration  of  [0°,  87°,  56°]  (shown  in  Figure 
7-19)  with  joint  1  actuating  at  1  Hz  and  joint  2  at  1.5  Hz.  The  resulting  interaction  forces 
along  with  predicted  total  forces  (inertia  plus  nonlinear)  in  the  y  directions  are  shown  in 
Figure  7-20.  The  total  interaction  forces  due  to  the  actuation  of  joint  1  is  small  in  this 
configuration,  so  the  force  in  the  y  direction,  as  expected,  is  relatively  small.  The  coriolis 
effects  are  expected  to  be  large  in  this  region,  which  would  result  in  combinations  of  the 
harmonics  at  approximately  .5  and  2.5  Hz,  in  this  case.  The  frequency  content  of  the 
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Force  y  (N) 


measured  force  data  is  shown  in  Figure  7-21  and  it  can  be  seen  that  and  the  higher 
harmonics  have  more  of  an  effect  here. 


Figure  7-19 

Anthropomorphic  Robot  Configuration  [0°,  87°,  56°] 


Time  (s) 


Figure  7-20 

Interaction  Forces  Due  to  Joints  1  and  2  Actuating  Near  an  Inertial  Singularity 
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Frequency  Content  of  Force  y  Data 


Figure  7-21 

Frequency  Content  of  Force  Data  due  to  Joints  1  and  2  Actuating  Near  an  Inertial 

Singularity 

As  an  example  of  a  region  where  nonlinear  centrifugal  forces  are  expected  to  be 
larger,  consider  the  configuration  [0°,38o,0°],  shown  in  Figure  7-22.  In  this 
configuration,  it  was  predicted  by  Figure  4-14  that  the  centrifugal  force  effects  would  be 
greater.  The  resulting  force  data  in  the  x  direction  and  frequency  content  are  shown  in 
Figures  7-23  and  7-24.  Joints  1,  2,  and  3  are  actuating  harmonically  at  1,  1.5,  and  2  Hz, 
respectively.  In  this  case,  the  interaction  force  effects  in  the  x  direction  will  be  primarily 
due  to  joints  2  and  3.  As  discussed  previously,  joint  3  has  relatively  little  inertia  so  it  was 
expected  the  higher  harmonics  consist  mainly  of  3  Hz  content,  or  twice  the  frequency  of 
joint  2.  Now  consider  the  interaction  torques  and  associated  frequency  content,  as  shown 
in  Figures  7-25  and  7-26,  respectively.  Here  instead  of  the  twice  the  frequency  of 
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actuation,  the  higher  harmonics  consist  of  the  combination  of  frequencies,  indicative  of 
increased  coriolis  effects,  as  expected. 


Figure  7-22 

Anthropomorphic  Robot  Configuration  [0°,38°,0°] 


Tlmo  (*) 


Figure  7-23 

Interaction  Forces  with  Large  Centrifugal  Effects  Due  to  Joints  1,  2,  and  3  Actuating 

Simultaneously 
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x  105  Frequency  Content  of  Torque  x  Data 


Figure  7-26 

Frequency  Content  of  Interaction  Torque  Data  Due  to  Joints  1, 2,  and  3 


There  are  two  main  conclusions  to  be  drawn  from  this  portion  of  the  experimental 
work.  First,  the  form  of  the  interaction  forces  and  torques  given  by  equation  4-1  is 
valid,  regardless  of  the  rigid  robot  dynamics.  In  the  case  of  this  experimental  work,  the 
joint  torque  dynamics  are  dominated  by  the  hydraulic  servovalves  and  are  hence  very 
different  from  the  ideal  robot  model  cited  in  most  robotics  texts.  However,  the  model 
for  the  interaction  forces  and  torques  is  still  valid.  The  second  important  point  is  that, 
regardless  of  the  orientation  of  the  robot,  higher  (and  sometimes  lower)  harmonics  will 
be  generated  by  the  robot.  If  higher  system  modes  exist,  which  will  be  the  case  with  a 
continuous  system,  higher  frequency  modes  can  become  excited.  However,  the  relative 
importance  of  the  harmonics  varies  throughout  the  workspace.  With  a  good 
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understanding  of  these  issues,  either  these  regions  can  be  avoided  or  the  control  gains 
selected  to  ensure  overall  energy  removal  from  the  system.  Note  this  part  of  the  work 
has  assumed  constant  joint  amplitudes,  and  the  ratio  of  the  inertia  to  nonlinear  effects 
will  improve  as  the  vibration  is  reduced.  The  next  step  was  to  proceed  forward  with 
implementation  of  the  vibration  controller  described  in  Chapter  5. 

7.4  Multi-Degree  of  Freedom  Vibration  Damping 
The  final  goal  of  this  research  was  to  extend  the  inertial  damping  control  scheme  to 
multiple  degrees  of  freedom.  This  is  a  challenging  extension  from  previous  work  at 
Georgia  Tech  for  two  main  reasons.  First,  the  testbed  now  has  highly  coupled  modes  and 
general  system  motion  in  many  directions  is  possible,  as  discussed  in  Section  7.2.2. 
Previous  testbeds  at  Georgia  Tech  demonstrated  clear  decoupled  vibration  i.e.  the  two 
transverse  fundamental  modes  of  vibration  of  the  beam  were  at  different  frequencies  and 
there  was  little  coupling  between  the  two.  Second,  the  use  of  three  links  of  the 
micromanipulator  requires  cooperation  between  joints  because  links  2  and  3  of  the 
testbed  robot  both  create  interaction  forces  and  torques  in  the  same  plane.  Hence,  their 
movements  have  to  be  cooperatively  prescribed  in  order  to  damp  the  motion  because 
damping  motion  by  either  one  requires  motion  in  the  other.  This  forces  accurate 
modeling  and  a  better  understanding  of  issues  that  may  result  when  the  robot  model 
contains  inaccuracies. 

The  block  diagram  for  the  multi-degree  of  freedom  case  can  be  seen  in  Figure  7-27. 
The  parameters  for  the  assumed  rigid  robot  model  were  given  in  Table  7-1  and  the 
feedforward  and  drift  compensators  were  given  in  Table  7-2  (only  the  first  three  joints  are 
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shown  in  the  block  diagram  below  since  they  are  the  only  joints  used  for  inertial 
damping).  The  rigid  robot  PI  gains  chosen  were  given  in  Table  7-3.  The  ID  (inverse 
dynamics)  function  is  defined  as: 

ID  =  BtB~/(0)  (7-7) 

IDjj  in  figure  7-27  is  the  ith  row  and  jth  column  of  the  resulting  matrix.  The  rigid  robot 
actuators  respond  as  a  near  velocity  source  so  BT  is  given  by: 


5  = 


K 


h2 


K 


h3 


(7-8) 


Bf(0)  is  given  by  equation  4-4  and  0  are  the  actual  robot  joint  angles  measured  from 
optical  encoders. 

The  scheme  relies  on  the  inverse  dynamics  function  to  cancel  the  rigid  robot  and 
coupling  effects  and  assumes  the  inertia  effects  are  the  most  significant  coupling  effects. 
As  discussed  in  sections  4.3  and  7.3.1,  the  inertia  effects  are  dominant  in  the  single 
degree  of  freedom  case.  However,  nonlinear  effects  can  become  more  significant  with 
multiple  links  actuating  harmonically.  Thus,  the  controller  also  relies  on  the  gain  limits 
established  in  section  5.3.1  to  limit  the  joint  amplitudes  such  that  the  inertia  effects 
adequately  capture  the  significant  interaction  effects. 
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Figure  7-27 


Block  Diagram  of  Multi-Degree  of  Freedom  Vibration  Damping 
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7.4.1  Disturbance  Rejection 


First,  the  vibration  controller  was  tested  in  a  configuration  of  (0°,  45°, -45°)  and  an 
excitation  was  applied  to  the  macromanipulator.  The  first  case  considered  was  with  the 
vibration  controller  turned  on  during  the  free  vibration,  and  the  resulting  base  vibration 
and  joint  motion  recorded.  The  second  case  was  with  the  vibration  controller  on  and  a 
disturbance  is  applied  to  the  macromanipulator.  A  summary  of  the  test  cases  and 
calculated  damping  ratios  is  shown  in  Table  7-8.  In  most  cases,  two  tests  were  performed 
for  each  case  and  the  damping  ratios  were  averaged.  In  nearly  all  cases,  as  expected, 
multiple  modes  were  excited,  but  the  lower  frequency  modes  were  those  responsible  for 
the  largest  amplitude  of  vibration.  In  all  of  the  test  cases  vibration  energy  is  removed 
from  the  overall  system.  The  damping  controller  performance  works  best  for  damping 
the  fundamental  mode  at  1 .2  Hz,  which  is  the  transverse  mode  of  the  first  link  in  the  y 
direction.  The  controller  gains  for  x  direction  vibration  were  selected  to  be  low 
compared  to  the  y  gains  in  order  to  help  avoid  excitation  of  the  higher  frequency  modes, 
notably  modes  at  4.2  and  9  Hz.  A  representative  example  of  the  recorded  base  position 
and  joint  motion  can  be  seen  in  Figures  7-28  and  7-29. 
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Table  7-8 


Summary  of  Active  Damping  Control] 

er  Disturbance  Rejection 

Disturbance 

Applied 

Mode 

(Hz) 

Detected 
Primarily  in 
Directions 

% 

Improvement 

X 

1 

X 

.0239 

28.5 

X 

1.2 

y,z 

.0392 

485.1 

X 

2 

x,y,z 

.0231 

X 

4.2 

X,Z 

.0164 

88.5 

y 

1 

X,Z 

.0499 

168.3 

y 

1.2 

y,z 

.0332 

395.5 

y 

4.2 

x,z 

.0111 

27.6 

y 

7 

y,z 

.0132 

157.3 

y 

9 

y,z 

.0081 

188.3 

Z 

1 

X 

.0311 

67.2 

Z 

1.2 

y,z 

.0419 

525.3 

z 

4 

x,z 

.0163 

87.4 

z 

7 

y,z 

224.6 

z 

9 

y,z 

191.8 

General 

1 

X 

37.6 

General 

1.2 

y,z 

.0359 

435.8 

General 

4.2 

X,Z 

.0120 

37.9 

General 

7 

y,z 

284.0 

General 

9 

y,z 

206.0 
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7.4.2  Combined  Movement  and  Vibration  Control 


The  second  set  of  experiments  was  intended  to  verify  the  use  of  the  performance 
index  to  predict  regions  of  good  inertial  damping  performance.  The  robot  was 
commanded  to  move  from  a  configuration  of  (-27°, 0°, 83°, 0°, 30°, 90°)  to 
(45°, 5°, 60°, 0°, 90°, 90°),  which  is  one  of  the  legs  presented  in  section  6.3.4  for 
simulated  point-to-point  motion  in  a  configuration  predicted  better  for  inertial 
damping  performance.  The  resulting  base  vibration  and  commanded  joint  movement 
may  be  seen  in  Figures  7-30  and  7-31.  Note  the  very  low  amplitude  of  vibration  in 
the  x  and  z  directions  compared  with  vibration  in  the  y  direction.  The  base  vibration 
with  and  without  the  vibration  controller  for  the  robot  moving  from  (-27°, 83°,- 
1 00°, 0°, 90°, 90°)  to  (45°, 65°, -60°, 0°, 90°, 90°),  which  is  the  alternate  inverse 
kinematics  path,  may  be  seen  in  Figure  7-32.  As  expected,  lower  amplitude  of 
vibration  results  in  the  regions  of  low  coupling,  but  the  vibration  controller  is  much 
less  effective.  The  tradeoff  in  the  better  configuration  is  the  impact  on  joint 
positioning,  since  the  controller  requires  small  joint  movements  about  the  desired  end 
point  position.  These  occurs  primarily  at  the  end  of  the  path,  although  there  are  a  few 
correctional  movements  made  just  before  the  end  of  the  trajectory  which  helps  reduce 
the  level  of  vibration. 
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Figure  7-30 

Base  Vibration  Due  to  Commanded  Movement 


Joint  1  Response 


Figure  7-3 1 

Joint  Motion  During  Commanded  Point-to-Point  Movement  with  Vibration  Control 
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Figure  7-32 

Base  Vibration  Due  to  Commanded  Movement  in  Region  with  Predicted  Poor  Inertial 

Damping  Performance 


7.5  Implementation  Issues 

Many  implementation  issues  arose  with  the  experimental  testing  of  the  control 
scheme  in  multiple  degrees  of  freedom.  These  will  be  discussed  along  with 
recommendations  for  improving  the  testbed  for  future  work. 


7.5.1  Acceleration  Data 

The  available  vibration  measurements  were  PCB  piezoelectric  accelerometers.  The 
fundamental  frequencies  of  the  testbed  were  very  low,  approximately  .98  and  1.2  Hz.  For 
the  model  3  03 A  accelerometers,  used  for  x  and  y  vibration  measurements,  the  +/-  5% 
frequency  range  of  these  accelerometers  is  from  1  -  10000  Hz  and  for  the  model  308B 
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accelerometers  is  from  1  -  3000  Hz  [57].  The  modes  of  vibration  on  the  new  testbed 
were  very  low  and  the  accelerometers  were  operating  toward  the  lower  limit  of  their 
useful  range.  This  was  further  complicated  by  the  beating  phenomenon  between  the  first 
two  fundamental  modes  discussed  in  Section  7.2.2.  In  addition,  ideally,  the  measured 
accelerations  would  need  to  be  integrated  as  noted  in  section  5.4.5.  As  noted  in  several 
sources  [66,84],  integration  of  piezoelectric  acceleration  time  histories  generally  results 
in  calculated  displacements  that  are  dominated  by  large,  low  frequency  drifts  unless  low 
frequency  spectral  content  is  filtered  out.  In  the  case  of  the  experimental  hardware,  it  is 
extremely  important  to  get  the  fundamental  (low)  frequency  modes  of  vibration  and  any 
filtering  would  need  to  be  performed  real-time  for  use  in  the  control  scheme.  The  use  of 
a  finite  impulse  response  filter  could  be  designed  to  avoid  introduction  of  phasing,  but 
not  implementable  real-time.  The  use  of  an  infinite  impulse  response  or  analog  filter 
could  be  used  but  higher  order  filters  would  be  needed  to  provide  a  sharp  enough  cutoff 
to  avoid  attenuation  of  the  low  frequency  modes. 

For  lab  implementation,  a  second  order  high  pass  digital  Butterworth  filter  with  a 
cutoff  frequency  of .  1 5  was  used.  This  was  necessary  due  to  offsets  in  the  accelerometer 
data  and  it  added  little  phase  shift  into  the  fundamental  modes.  For  this  work,  it  was 
considered  a  reasonable  approximation  to  use  the  measured  acceleration  data  and  it 
provided  reasonable  performance.  For  future  work,  it  is  recommended  that  direct 
position  measurements  be  used  that  allow  for  very  low  frequency  vibration  measurement. 
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7.5.2  Hydraulics  Effects 


As  discussed  in  Sections  6.4  and  7.2.1,  it  was  assumed,  and  experimentally 
verified  by  open  loop  testing,  the  hydraulic  actuators  followed  a  model  of  the  form  of 
Equation  7-2.  Figure  7-33  shows  the  commanded  output  from  the  PC  (unitless)  and  the 
resulting  joint  response  at  a  low  frequency  command.  The  response  is  very  nearly  like  an 
integrator,  as  expected.  The  same  comparison  for  a  higher  frequency  commanded  output 
is  shown  in  Figure  7-34,  which  shows  much  poorer  tracking  at  higher  frequencies.  The 
second  order  servovalve  models  and  parameters  in  Table  7-1  do  not  predict  this  behavior. 
Second  order  servovalve  models  were  noted  by  the  vendor  as  being  a  good 
approximation  at  frequencies  lower  than  50  Hz  [75,76].  However,  the  observed  behavior 
indicates  that  a  higher  order  system  model  is  required  for  accuracy.  Merritt  [50] 
recommends  a  third  or  fourth  order  system  model,  depending  on  the  accuracy  of  the 
model  required.  For  future  work  it  is  recommended  that  higher  order  servovalve  models 
be  developed  from  system  testing.  Better  high  frequency  response  could  then  be  obtained 
by  developing  phase  compensators  from  the  known  models. 
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7.5.3  Joint  Torque  Effects 


The  vibration  controller  gives  reasonably  good  performance  in  approximately  half 
of  the  workspace.  Whenever  the  robot  is  facing  away  from  the  macromanipulator,  the 
controller  does  not  function  properly  to  remove  energy  from  the  system  (Figure  7-35). 
This  is  due  to  a  combination  of  two  reasons.  First,  joint  1  provides  a  direct,  amplified 
input  into  the  torsional  mode  of  the  first  link  in  these  configurations.  As  discussed  in 
Section  6.3.2,  when  the  interaction  torques  are  not  considered  it  makes  little  difference  in 
the  controller  performance  in  some  workspace  configurations.  However,  in  the 
configuration  shown  in  Figure  7-35,  the  interaction  torques  have  a  much  greater  effect  on 
the  performance  of  the  vibration  controller  and  should  be  included  in  the  controller.  This 
effect  is  magnified  even  more  on  the  testbed  due  to  the  additonal  leverage  provided  by 
the  second  flexible  link.  Given  the  mode  shape  equation  in  3-9,  the  torsional  mode  shape 
is  largest  at  the  tip  of  the  beam,  hence  application  of  the  torque  here  has  the  greatest 
effect  in  exciting  it.  This  direct  excitation  combined  with  poor  high  frequency  tracking, 
as  discussed  in  Sections  6.4  and  7.5.2,  rendered  these  configurations  unusable  for  active 
damping  on  the  current  testbed. 

As  an  example,  consider  the  top  view  of  the  macro/micromanipulator  shown  at  the 
bottom  of  Figure  7-35.  The  figure  on  the  left  shows  a  configuration  of  poor  inertial 
damping  performance,  while  the  figure  on  the  right  shows  a  configuration  in  which  the 
controller  performed  better.  Recall  the  generalized  forces  due  to  the  micromanipulator 
are  given  by  equation  3-17.  In  the  case  on  the  left,  the  interaction  forces  remove 
vibrational  energy  from  the  system,  but  the  torques  may  add  energy.  In  the  case  on  the 
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right,  both  the  interaction  forces  and  torques  remove  vibrational  energy  from  the  system. 
This  further  reiterates  the  need  for  including  interaction  torques  in  the  control  scheme. 


CHAPTER  VIII 


CONCLUSIONS 

7.1  Conclusions 

This  dissertation  has  developed,  simulated,  and  demonstrated  a  position  and 
enhanced  vibration  control  scheme  for  a  macro/micromanipulator.  The  analogous  case  of 
a  rigid  manipulator  attached  to  a  flexible  but  unactuated  base  was  used  to  study  the 
directly  controllable  inertial  interaction  forces  and  torques  acting  between  the  robot  and 
its  base.  The  “inertial  singularities”  in  the  joint  workspace  were  investigated  in  detail, 
namely  the  regions  where  the  robot  loses  its  ability  to  create  interactions  in  one  or  more 
degrees  of  freedom.  A  performance  index  was  developed  to  predict  the  ability  of  the 
robot  to  generate  interactions  and  can  be  used  to  ensure  the  robot  operates  in  joint  space 
configurations  favorable  for  inertial  damping.  It  was  shown  that  when  this  is  used  along 
with  the  appropriate  choice  of  vibration  control  feedback  gains,  the  inertia  effects,  or 
those  directly  due  to  accelerating  the  links  of  the  robot,  have  the  greatest  influence  on  the 
interactions.  By  commanding  the  link  accelerations  proportional  to  the  base  vibration, 
energy  will  be  removed  from  the  system.  The  vibration  control  signal  is  added  to  the 
position  control  signal.  Simulated  and  measured  interaction  forces  and  torques  generated 
at  the  base  of  a  rigid  robot  were  compared  to  verify  conclusions  drawn  about  the 
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controllable  interactions.  In  addition,  simulation  and  experimental  results  demonstrated 
the  combined  position  control  and  vibration  damping  ability  of  the  scheme. 

The  true  contributions  began  in  Chapter  4  with  a  detailed  discussion  of  the 
controllable  interaction  force  and  torques.  A  performance  measure  was  introduced  which 
predicts  the  effectiveness  of  the  rigid  robot  in  creating  these  interactions.  The  rigid 
inertia  effects  (Bf,  Bt0)  were  studied  in  more  detail  and  the  “inertial  singularities” 
investigated.  It  was  pointed  out  that  these  singularities  are,  in  general,  different  from  the 
kinematic  singularities.  However,  since  the  variation  in  performance  is  governed  by  the 
joint  space  configuration  of  the  rigid  robot,  the  performance  measure  could  be  used  to 
quickly  assess  predicted  inertial  damping  performance  at  the  multiple  inverse  kinematic 
joint  space  solutions  and  used  to  operate  the  rigid  robot  in  configurations  better  suited  for 
inertial  damping. 

The  inertia  effects  dominate  the  interactions  in  most  non-singular  configurations,  but 
in  certain  cases,  the  nonlinear  rigid  effects  may  also  become  significant  and  these  cases 
are  discussed.  The  control  scheme  was  discussed  in  Chapter  5.  The  vibration  controller 
requires  an  inverse  dynamics  function  to  cancel  the  most  important  coupled  interaction 
effects.  Guidelines  on  choosing  controller  gains  to  ensure  the  inertia  effects  are  the 
dominant  terms  were  presented. 

Simulation  results  were  presented  in  Chapter  6  demonstrating  vibration  damping  of  a 
three  degree  of  freedom  anthropomorphic  robot  mounted  on  a  flexible  base.  Simulations 
demonstrating  disturbance  rejection  as  well  as  the  use  of  the  performance  index  in 
predicting  better  joint  space  configurations  for  vibration  damping  during  commanded 
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motion  were  included.  Chapter  7  discussed  the  experimental  testbed  and  presented 
results  from  two  areas  of  testing.  The  first  was  verification  of  the  interaction  force  and 
torque  effects  and  work  discussed  in  Chapter  4.  The  second  was  implementation  of  the 
vibration  control  scheme  on  a  multi-degree  of  freedom  experimental  testbed.  Although 
there  were  problems  that  arose  with  damping  in  certain  workspace  configurations,  in 
most  of  the  workspace  the  vibration  controller  performed  well  to  remove  energy  from  the 
system.  This  included  both  disturbance  rejection  as  well  as  vibration  control  during 
commanded  movements.  Finally,  implementation  issues  were  discussed. 

7.2  Contributions 

Contributions  of  this  work  are: 

1.  Extension  of  the  macro/micromanipulator  control  problem  to  multiple  degrees  of 
freedom  by  considering  the  analogous  problem  of  a  rigid  manipulator  mounted  on  a 
flexible  base. 

2.  Detailed  investigation  of  inertial  singularities  and  variation  in  performance 
throughout  the  workspace  and  development  of  a  performance  measure  that  predicts 
the  ability  of  the  micromanipulator  to  effectively  damp  base  vibration  using  an 
inertial  damping  scheme. 

3.  Development  of  a  control  scheme  that  provides  active  base  vibration  damping  in 
parallel  with  rigid  robot  position  control.  This  includes  the  use  of  the  performance 
index  to  improve  the  vibration  damping  capability  of  the  micromanipulator  given  a 
desired  end  point  position  and  establishing  appropriate  limits  on  control  gains. 
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4.  Verification  of  the  above  control  scheme  via  simulation 


5.  Verification  of  the  above  control  scheme  via  experimental  work.  This  included 
verification  of  the  accuracy  of  the  interaction  force  and  torque  predictions  and 
demonstration  of  the  effectiveness  of  the  control  scheme  on  a  realistic  multi-degree  of 
freedom  testbed. 

This  work  primarily  builds  on  that  performed  previously  by  Lee  [11,33],  Cannon 
[15,16],  Loper  [12,42],  and  Lew  [34-40].  This  extends  their  work  to  include  some 
concepts  introduced  in  space  robotics  research  by  Papadopoulos,  Evangelos,  and 
Dubowsky  [56]  and  Torres  and  Dubowsky  [77-79].  In  particular,  this  is  the  first  work  to 
develop  the  complete  interaction  forces  and  torque  effects  for  a  macro/micromanipulator 
and  investigate  the  variation  in  performance  throughout  the  workspace.  In  addition,  this 
is  the  first  work  to  introduce  “inertial  singularities,”  determine  when  and  why  they  occur, 
and  propose  a  solution  to  work  around  them.  This  is  also  the  first  work  to  consider 
multiple  modes  of  vibration  and  propose  control  gain  limits  to  ensure  overall  energy 
removal  from  the  system.  Finally,  this  is  the  first  work  demonstrating  multiple-degrees 
of  freedom  of  vibration  damping  on  an  experimental  testbed,  involving  multiple  joints 
actuating  cooperatively  for  vibration  damping.  Although  the  scheme  could  not  be  shown 
to  be  effective  in  all  configurations,  mainly  due  to  implementation  issues  and  simplifying 
assumptions  made  to  scale  the  inverse  dynamics  function  to  an  implementable  level,  this 
work  demonstrates  a  clear  contribution  to  the  field. 
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7.3  Recommendations  for  Future  Research 


There  are  many  areas  of  research  that  could  be  further  investigated  including 
implementation  issues  that  were  discussed  in  section  7.5.  As  noted  in  Chapters  6  and  7  a 
more  reasonable  method  of  modeling  the  interaction  torque  effects  is  needed.  The 
algorithm  described  in  Section  4.4  allowed  for  a  much  easier  derivation  of  the  interaction 
forces  than  the  symbolic  method  described  in  Section  3.3,  but  a  similar  method  was  not 
found  for  the  interaction  torques.  Mainly,  the  length  of  the  interaction  torque  equations, 
detailed  in  Section  A.  1.2  for  the  three  degree  of  freedom  case,  made  simulating  and 
studying  them  in  detail  cumbersome  and  unimplementable  in  practice.  More  work  needs 
to  be  done  in  this  area,  either  to  discover  a  much  more  efficient  method  of  deriving 
expressions  for  the  interaction  torques  or  eliminating  effects  that  are  less  important. 

The  use  of  input  shaping  in  combination  with  an  inertial  damping  control  scheme  is 
an  area  worthy  of  future  research.  Input  shaping  techniques  are  effective  at  reducing  the 
amplitude  of  vibration  induced  by  commanded  movements  but  typically  do  not  add 
damping  to  the  system.  This  becomes  particularly  noticeable  in  an  example  such  as  that 
shown  in  section  6.3.4  and  7.4.2,  where  damping  was  compared  for  commanded  rigid 
robot  movement  with  and  without  inertial  damping.  If  implemented  with  input  shaping, 
vibration  created  by  motion  of  the  robot  itself  would  be  reduced  and,  assuming  the  robot 
is  operating  in  a  region  of  predicted  good  performance,  the  controller  could  still  provide 
disturbance  rejection.  Hence,  the  combination  of  the  two  schemes  should  provide  the 
best  performance  for  both  commanded  movement  vibration  control  as  well  as  disturbance 
rejection. 


213 


Another  area  worthy  of  future  research  is  the  effect  of  implementation  of  the  control 
scheme  on  a  digital  controller.  Simulations  as  well  as  experimental  work  were  performed 
using  continuous  system  theory.  Implementation  involved  real-time  calculation  of  the 
control  signals  during  each  control  loop,  based  on  sampling  the  appropriate  control 
signals  (positions  and  accelerations).  However,  with  increased  computation  time,  which 
will  occur  especially  if  interaction  torques  are  included,  sample  and  hold  effects  as  well 
as  finite  precision  effects  may  become  more  of  an  issue. 

Another  area  that  should  be  considered  is  implementation  of  the  proposed 
controller  with  a  macromanipulator  at  varying  joint  angles.  This  would  change  the 
natural  frequencies  and  damping  ratios  of  the  modes  of  vibration,  as  well  as  the 
orientation  of  the  applied  interaction  forces.  The  proposed  controller  should  theoretically 
apply  as  long  as  the  frequency  of  vibration  is  measurable,  but  other  unforeseen  problems 
could  occur. 

As  discussed  in  section  6.3.1,  vibration  control  performance  is  not  as  efficient  in 
regions  of  high  nonlinear  effects  when  higher  modes  exist  in  the  system,  which  requires 
lower  control  gains.  Performance  could  possibly  be  improved  by  canceling  those  effects 
in  the  vibration  control  scheme  or  using  some  alternate  controller  form.  Another  area  of 
future  research  should  consider  the  effect  of  contact  on  the  tip  of  the  micromanipulator. 
The  work  performed  thus  far  assumes  no  contact  on  the  system,  whereas  the  more 
general  case  would  likely  involve  contact  by  the  micromanipulator.  Appropriate 
modifications  to  the  controller  and/or  inverse  dynamics  function  may  be  necessary  in 
these  cases. 
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APPENDIX  A 


EQUATIONS  OF  MOTION 


For  completeness,  this  appendix  includes  the  symbolic  form  of  the  fully  coupled 
equations  of  motion  for  the  anthropomorphic  robot  as  well  as  some  of  the  more  important 
equations  for  the  other  configurations  studied  in  this  work.  As  discussed  in  chapter  four, 
some  of  these  effects  are  more  significant  than  others.  The  general  form  of  the 
interaction  forces  and  torques  and  notation  used  is: 
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T=BT(e,d)+NRT(eAdj)+NCT(e,^)+GT(e)+AT(9,q)+NRCT(eM)+NCCT(e,^) 
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where: 


B  matrices  are  rigid  body  inertia-like  matrices  (Bt  is  the  inertia  matrix) 

Nr  matrices  are  rigid  body  coriolis  effects 
Nc  matrices  are  rigid  body  centrifugal  effects 
G  matrices  are  rigid  body  gravity  effects 
A  are  flexible  body  inertia  matrices  (due  to  base  acceleration) 

Nrc  are  flexible  body  coriolis  effects  (due  to  base  rotational  velocities) 

Ncc  are  flexible  body  centrifugal  effects  (due  to  base  rotational  velocities) 

Bw  are  flexible  body  rotational  inertia  matrices  (due  to  base  rotational  accelerations) 
Ncm  are  cross  coupling  effects  due  to  both  joint  velocities  and  base  rotational  velocities 


This  notation  will  be  used  for  all  robot  configurations  studied  in  this  work.  As 
discussed  in  Chapter  3,  equations  were  derived  from  a  Matlab  m-file  using  a  Newton 
Euler  method.  Due  to  their  length  and  complexity,  great  pain  was  taken  to  find  forms 
that  are  more  useful  for  the  critical  rigid  and  flexible  inertia  and  nonlinear  rigid  effects. 
However,  due  to  the  complexity  of  the  equations  this  was  only  done  for  the  some  of  the 
equations.  The  remainder  are  included  in  the  general  (“simple”  format)  form  provided  by 
Matlab. 


A.l  Three  Degree  of  Freedom  Anthropomorphic  Robot 
The  anthropomorphic  robot  configuration  used  here  is  similar  to  that  used  in 
experimental  work.  The  same  link  parameters  and  Denavit-Hartenberg  parameters  were 
used  as  for  the  actual  robot  and  the  parameters  are  defined  Figure  4-1.  Notice  the 
alternate  definition  of  joint  angle  two.  This  was  chosen  because  software  code  was 
already  written  to  control  the  robot  using  these  parameters  and  it  avoided  numerous 
changes  to  the  software  code.  The  Denavit  and  Hartenberg  parameters  are  shown  in 
Table  A-l. 
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Table  A-l 


Denavit-Hartenberg ' 

Parameters  for  Anl 

hropomorphic  Robot 

Link 

af 

Otj 

di 

0, 

0 

0 

0 

do 

inert 

1 

0 

7l/2 

di 

e, 

2 

a2 

0 

0 

©2 

3 

&3 

0 

0 

03 

Rotation  Matrices 


*>° 

R\ 

R  3 


cos(0 ,)  0  sin(0,) 

sin(#,)  0  -cos(0,) 

01  0 
cos(02 )  -sin(#2)  0 

sin(02)  cos(02)  0 

001 
cos (03)  -sin(0})  0 

sin(03)  cos(03)  0 

0  0  1 


(A-2) 


It  will  be  useful  to  define  some  constants  to  help  simplify  the  equations  of  motion: 


A  =  m2r2  +  m3a2 
B  =  m3r3 
D  = 

E  =  m3r3 
F  =  m3a2 

G  =  I2xx  ~  I 2  yy 

H=hzz+hiz 

K  =  m2r2  +  m3a\ 

L  =  Ilyy+I2xx+I3yy 
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M  =  m0  +  mx  +  m2  +  m3 
N  =  m3a2r3 

O  =  m0r0  +  mx  ( d0  +  rx )  +  (m2  +  m3  )D 

P  ~  -hxx  +  hzz  +  hzz  +  hzz 

Q  ^2  XX  +  hyy+hxx+hyy 


Postion  Vector  to  CG 


~  M 


Cj(Ac2  +  Bc23) 
S](Ac2  +  Bc23) 
O + As2  +  Bs23 


(A-3) 


(A-4) 


A.  1.1  Interaction  Force  Effects 


Rigid  Inertia  Forces 


-sx(Ac2  +Bc23 ) 

-cx(As2  +  Bs23  ) 

BC\S2?> 

II 

cx(Ac2  +Bc23) 

-5,(^2 +R523) 

-Bs,s23 

0 

v4c2  +  Bc23 

BC2i 

Bf  =  -ABs3(Ac2  +  Bc23) 


Rigid  Coriolis  Forces 

2sx  (As2  +  Bs23  )  2Bsxs23  -  2Bcxc23 

-2c,  (As  2  +  2Bs23  )  -  2  Bcxs23  -2Bsxc23 

0  0  -  2  Bs23 


(A-5) 


(A-6) 


Rigid  Centrifugal  Forces 


f’i  (Ac  2  +  Bc23) 
-sx(Ac2+Bc23) 
0 


C\  (Ac2  +  Bc23  )  Bcxc23 

-sx  (Ac2+  Bc23  )  -  Bsxc23 

-(As2+Bs23 )  -R523 


(A-7) 
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Rigid  Gravitational  Forces 


0 

0 


Mg 


Flexible  Body  Inertia  Forces 


M 

0 

0 


0  0 
M  0 
0  M 


Flexible  Body  Coriolis  Forces 


N RCf  (b  1)  —  -Vi  (Ac 2  +  Bc22  ) 

NRCf(l,2)  =  0  +  As2+Bs23 

NRCf(  1,3)  =  NRCf(2, 2)  =  NRCf(3,Y)  =  0 

N Re/  (2»  1)  =  Cj  (Ac2  +  Bc23) 

Nrc/  (2>  3)  =  O  +  As  2  +  Bs23 
A*c/(3,2)  =  c \(Ac2+Bc23) 

N Rcf  (3, 3)  =  Sj  ( Ac2  +  Bc23  ) 


Flexible  Body  Centipetal  Forces 

ACC/(U)  =  NCCf( 2,2)  =  Nccf(  3,3)  =  0 
NCCf  (!» 2)  =  ~<h  (Ac2  +  Bc23  ) 

A ccf  0-»  2)  ~  (Ac2  +  Bc23  ) 

NCCf  (2)  1)  =  — ^1  (-^Cj  +  -®C23  ) 

NCCf(  2>3)  =  — -S]  (Ac2  +  Bc23  ) 

N ccf  (3)  1)  =  (O  +  As  2  +  Bs23  ) 

^ CCf  (3;  2)  =  —  (O  +  As2  +  5^23  ) 


(A-8) 


(A-9) 


(A- 10) 


(A- 11) 
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Flexible  Body  Rotational  Inertia  Forces 


^(U)=5l/(2,2)=5l/(3,3)=0 

5nf(U)  =0+ A +&23 

=~(0+As2  +Bs23) 

+BC2l) 

Bh/Q,1)=sx(Ac2+Bc23) 

5^3,2)=^+^)  (A-12) 


Cross  Coupling  Force  Effects 
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2  c\  (Ac 2  +  Bc23 )  (Ac2  +  Rc23  ) 
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~2Bc1s23 


2Cj(Ac2  +Bc23) 
2sj(Ac2+Bc23) 
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—  2s,  (As2  +  Bs23  ) 
2c,  ( As2  +R523) 
0 

-2Bsxs23 
2  Bcxs23 
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ir 


(A- 13) 


A.l  .2.  Interaction  Torque  Effects 
Rigid  Inertia  Interaction  Torques 

2?r0(l,l)  =  c,{-2c2s23[c3(£'  +  J)  +  Ba2]  +  (E  +  J)(s2c2  +  s3c3)  +  B(a2s3  -Dc23)  +  (G-K)s2c2-ADc2} 
Bz0(  1, 2)  =  5,  (fi(2a2c3  +  Z)s23)  +  +  (£  +  if  +  £)} 

Bv  o  (f3)  =  sx  {B(a2c3  +  Ds23  )  +  E  +  I3zz) 

Bt0( 2,1)  =  5,  {-2c2523[c3 (£  +  J)  +  5a2 ]  +  (£  +  J)(s2c2  +  s3c3 )  +  B(a2s3  - Dc23 )  +  (G-  K)s2c2  -  ADc2 } 
Bl0  (2, 2)  =  -c,  {5(2a2c3  +  Ds23)  +  ADs2  +(E  +  H  +  K)} 

Br0  (2,3)  =  -c,  {B(a2c3  +  Ds23  )  +  (E  +  I3zz)} 

5r0  (3, 1)  =  2c2c23  [c3  (E  +  J)  +  Ba2  }-(E  +  J)(c22  +  c2)  +  (K-G)c22+E  +  L 

5r0(3,2)  =  5r0(3,3)  =  0  (A-14) 
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Rigid  Coriolis  Interaction  Torques 


NRt  o  (1,1)  =  c,  {-4  c2c23  [c3  (£  +  ./)  +  5a2  ]  ■ +  2(5  +  J)(c\  +  c2)  +  2B(2a2c3  +  Ds23 ) 

+  2{G  -  K)c\  +  2ADs2+2K  +  H -G  -  J) 

NRt0  (1, 2)  =  c,  {-2c2c23  [2c3  (E  +  J)  +  Ba2  ]  +  2  (E  +  J)(c22  +  c2 )  +  25(a2c3  +  £>s23 ) 

NRz0 (1,3)  =  -2Bsl  {a2s3  -  Dc23 } 

NRt0  (2,1)  =  5,  {-4c2c23  [c3  (E  +  J)  +  Ba2  ]  +  2(5  +  J)(c2  +  c3  )  +  2B(2a2c3  +  Ds23 ) 

+  2(G  -  K)c22  +  2v4Z)52  +  2K  +  H-G-J} 

NRt0  (2, 2)  =  s,  {-2c2c23[2c3  (E  +  J)  +  Ba2  ]  +  2{E  +  J)(c2  +  c3)  +  2B(a2c3  +  Ds23 ) 

NRt0  (2, 3)  =  25c,  {a2s3  -  Dc23 } 

A5r0  (3, 1)  =  2  {-2  c2s23  [c3  ( E  +  J)  +  Ba2  ]  +  (5  +  J)(s2c2  +  s3c3 )  +  Ba2s3  +  (G  -  K)s2c2 } 

NRt0  (3, 2)  =  2{-c2s23[2c3(5  +  /)  +  Ba2]  +  (E  +  J)(s2c2  +  s3c3)} 

NRt0(  3,3)  =  0  (A- 15) 


Rigid  Centrifugal  Interaction  Torques 

4,0 (1,1)  G  {2^2^23 [^3 0^  *0  Ba2 ]  ( E  +  J)(s2c2  +  s3c3 )  B(^q2s3  Dc23  ) 
~(G-K)s2c2  +  ADc2} 

4,0  O’ 2)  =  Dsi  {^2  +  Rc23  } 

4,o  0>  3)  =  -Bsx  {a2s3  -  Dc23  } 

4,o  (2»  1)  =  C\  2c2s23  [c3  (5  + «/)  +  2Ba2  ]  +  (5  +  ,/)(,s-2c2  +  53c3 )  +  B(a.2s3  —  Dc23 ) 

+  (G  -  K)s2c2  -  ADc2  } 

4,o  (2»  2)  =  —Dcx  {Bc23  +  v4c2} 

4,o  (2, 3)  =  5c,  (c253  -  5>c23 } 

4,oO»l)  =  4,o  2)  =  4,0  O’ 3)  =  0  (A- 16) 


Rigid  Gravitational  Interaction  Torques 


G„ 


gsi(Ac2  +5c23) 
- gci(Ac2+Bc23 ) 
0 


(A- 17) 
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Flexible  Body  Inertia  Interaction  Torques 


rO 


0  -  (O  +  As  2  +  Bs23)  sx(Ac2  +  Bc23 ) 

O  +  As  2  +  Bs23  0  -  cx  (Ac2  +  i?c23  ) 

-sx(Ac2  +  i?c23)  Cj(^4c2  +  i?c23)  0 


(A- 18) 


Flexible  Body  Coriolis  Interaction  Torques 


NRCx0(l5l):=-cos(thl)*(cos(th2)*I2yy*sin(th2)-sin(th2)*I2xx*cos(th2)+cos(th2)*I3xx*sm(th2) 

-  m3*sin(th3)*sin(th2)*rc3*dl+sin(th2)*m2*cos(th2)*rc2A2+m3*cos(th3)*cos(th2)*rc3*dl 
+2*sin(th2)*m3*rc3A2*cos(th3)A2*cos(th2)-sin(th2)*sin(th3)*m3*rc3*d0 
+2*m3*cos(th2)*rc3*cos(th3)*sin(th2)*a2+cos(th2)*cos(th3)*m3*rc3*d0 
+2*cos(th3)*m3*rc3A2*sin(th3)*cos(th2)A2“Sin(th2)*I3yy*cos(th2)-a2*rc3*sin(th3)*m3 
-2*cos(th3)*I3xx*sin(th3)*cos(th2)A2-2*sin(th2)*I3xx*cos(th3)A2*cos(th2) 
+2*sin(th2)*I3yy*cos(th2)*cos(th3)A2+m3*cos(th2)*a2*d0+m2*cos(th2)*rc2*dl 

-  cos(th2)*m3*rc3A2*sin(th2)+cos(th2)*m2*rc2*d0+sin(th2)*m3*cos(th2)*a2A2 
+m3*cos(th2)*a2*dl+2*sin(th3)*I3yy*cos(th3)*cos(th2)A2-sin(th3)*rc3A2*m3*cos(th3) 
-cos(th3)*I3yy*sin(th3)+2*m3*sin(th3)*rc3*a2*cos(th2)A2+sin(th3)*I3xx*cos(th3)) 

NRCt0(1?2)=  -cos(thl)*sin(thl)*(-m3*rc3A2+I3xx+I2yy-I3xx*cos(th3)A2-  I3xx*cos(th2)A2 

+I3yy*cos(th2)A2+I3yy*cos(th3)A2+m3*rc3A2*cos(th2)A2+m3*rc3A2*cos(th3)A2-m2*cos(th2)A2*rc2A2 

-2*I3yy*cos(th3)A2*cos(th2)A2-m3*a2A2*cos(th2)A2-I2yy*cos(th2)A2-2*m3*rc3A2*cos(th3)A2*cos(th2)A2 

+cos(th2)A2*I2xx+2*cos(th2)*sin(th3)*m3*rc3*sin(th2)*a2+2*cos(th2)*cos(th3)*I3yy*sin(th3)*sin(th2) 

-2*cos(th2)*sin(th3)*I3xx*cos(th3)*sin(th2)+2*I3xx*cos(th3)A2*cos(th2)A2 

+2*cos(th2)*sin(th3)*m3*rc3A2*cos(th3)*sin(th2)-2*m3*rc3*cos(th3)*cos(th2)A2*a2+Ilxx-I3zz-I2zz- 

Ilzz) 


NRCx0(l,3)=  -2*rcl*ml*d0+3/4*I3yy*cos(2*th3+2*th2)+3/4*m3*a2A2*cos(2*th2)-I0yy-2*m3*d0*dl 

+3/4*m2*rc2A2*cos(2*th2)+3/4*m3*rc3A2*cos(2*th3+2*th2)4/8*I2yy*cos(2*thl+2*th2)-  2*m2*d0*dl 

+l/4*I3xx*cos(2*thl)-  l/8*I3xx*cos(2*th2-2*thl+2*th3)+l/8*I3xx*cos(2*th24-2*thl+2*th3) 

-l/4*m3*a2A2*cos(2*thl)-l/8*m3*a2A2*cos(-2*thl+2*th2)-  l/8*m3*a2A2*cos(2*thl+2*th2) 

+l/8*I2xx*cos(2*thl+2*th2)-l/8*I2yy*cos(-2*thl+2*th2)+l/8*I2xx*cos(-2*thl+2*th2) 

+l/4*I2xx*cos(2*thl)-2*m2*rc2*d0*sin(th2)-2*in2*rc2*dl*sin(th2)-2*m3*a2*d0*sin(th2) 

-2*m3*a2*dl*sin(th2)-l/2*m3*rc3*cos(di3)*a2-3/4*I2xx*cos(2*th2)+l/4*I3xx+l/4*I2yy+l/4*I3yy 

-m0*rc0A2+Ilyy4/2*I2zz+l/4*I2xx-l/2*I3zz-l/2*Ilxx+I0zz-ml*rclA24/4*m3*rc3A2-l/2*Ilzz 

-m3*d0A2-ml*d0A2<m3*dlA2-l/4*m3*a2A2-l/4*m2*rc2A2-m2*d0A2-m2*dlA2 

-l/4*m3*rc3A2*cos(2*thl)-l/8*I3yy*cos(2*th2-2*thl+2*th3)-l/8*I3yy*cos(2*th2+2*thl+2*th3) 

4d/4*I3yy*cos(2*thl)4/4*m2*rc2A2*cos(2*thl)4/8*m2*rc2A2*cos(-2*thl+2*th2) 

-l/8*m2*rc2A2*cos(2*thl+2*th2)-l/4*m3*rc3*a2*cos(th3+2*th2+2*thl) 

-l/4*m3*rc3*a2*cos(th3+2*th2-2*thl)+3/2*m3*rc3*a2*cos(2*th2+th3)-2*m3*rc3*d0*sin(th3+th2) 

-l/4*m3*rc3*a2*cos(th3-2*thl)-l/4*m3*rc3*a2*cos(th3+2*thl)-2*m3*rc3*dl*sin(th3+th2) 

-l/8*m3*rc3A2*cos(2*th2-2*thl+2*th3)-l/8*m3*rc3A2*cos(2*th2+2*thl+2*th3) 

-l/2*I3zz*cos(2*thl)+l/2*Ilxx*cos(2*thl)-l/2*I2zz*cos(2*thl)-l/2*Ilzz*cos(2*thl) 


NRCx0(2Jl)=  sin(thl)*(m3*cos(th2)*a2*dl-cos(th2)*m3*rc3A2*sin(th2) 
+2*cos(th2)*I3yy*cos(th3)A2*sin(th2)+m3*cos(th2)*a2*d0+sin(th2)*m3*cos(th2)*a2A2 
-2*cos(th2)*I3xx*sin(th2)*cos(th3)A2+sin(th2)*m2*cos(th2)*rc2A2+m2*cos(th2)*rc2*dl 
-cos(th3)*m3*rc3A2*sin(th3)+m2*cos(th2)*rc2*d0+2*sin(th3)*I3yy*cos(th3)*cos(th2)A2 
-2*cos(th3)*I3xx*sin(th3)*cos(th2)A2-  sin(th3)*m3*rc3*a2+cos(th2)*I3xx*sin(th2) 
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+cos(th3)*I3xx*sin(th3)-sin(th3)*I3yy*cos(th3)+cos(th2)*I2yy*sin(th2)-sin(th2)*I2xx*cos(th2) 
+2*sm(th3)*in3*rc3*cos(th2)A2*a2+2*cos(th3)*rn3*rc3A2*sin(th3)*cos(th2)A2 
-m3*sin(th3)*sin(th2)*rc3*d0-  m3*sin(th3)*sin(th2)*rc3*dl+2*sin(th2)*m3*rc3A2*cos(th3)A2*cos(th2) 
+2*sin(th2)*cos(th3)*m3*rc3*cos(th2)*a2+in3*cos(th3)*cos(th2)*rc3*dl+rn3*cos(th3)*cos(th2)*rc3*d0 
-cos(th2)*I3yy*sin(th2)) 

NRCt0(2,2)=  -3/4*I2yy*cos(2*th2)+l/4*I2yy*cos(2*thl)+l/4*I3xx*cos(2*thl) 

-l/8*I2yy*cos(2*thl+2*th2)-l/8*I2yy*cos(-2*thl+2*th2)-l/8*m2*rc2A2*cos(2*thl+2*th2) 

-l/8*m2*rc2A2*cos(-2*thl+2*th2)-l/8*m3*a2A2*cos(2*thl+2*th2)-l/4*m3*a2A2*cos(2*thl) 

-l/4*m2*rc2A2*cos(2*thl)-l/8*m3*a2A2*cos(-2*thl+2*th2)-l/4*m3*rc3*a2*cos(th3+2*thl) 

-l/4*m3*rc3*a2*cos(th3-2*thl)+l/4*I3yy*cos(2*thl)+l/4*I2xx*cos(2*thl)+3/4*I3xx*cos(2*th3+2*th2) 

-1/4*13  xx-l/4*I2yy-l/4*I3yy-l/4*I2xx+3/4*I2xx*cos(2*th2)+l/2*cos(th3)*m3*rc3*a2 

+l/8*I2xx*cos(-2*thl+2*th2)+2*m2*d0*dl+2*rcl*ml*d0-Ilyy+l/2*Ilzz-  I0zz+l/2*Ilxx+m0*rc0A2 

+I0xx+2*sm(th2)*rc2*m2*d0+2*sin(th2)*m3*dl*a2+2*sin(lh2)*rc2*m2*dl+2*sm(th2)*m3*d0*a2 

+1/2*13  zz-3/4  *13  yy*cos(2*th3+2*th2)+l/8*I2xx*cos(2*thl+2*th2)+l/4*m3*a2A2+m2*dlA2+ml*rclA2 

+m3*dlA2+ml*d0A2+m3*d0A2+m2*d0A2+l/2*I2zz+l/4*m2*rc2A2+2*m3*d0*dl-l/2*I2zz*cos(2*thl) 

+l/2*Ilxx*cos(2*thl)+l/4*m3*rc3A2-l/4*m3*rc3A2*cos(2*thl)-l/2*Ilzz*cos(2*thl) 

-3/4*m3*rc3A2*cos(2*th3+2*th2)-3/4*m3*a2A2*cos(2*th2)+l/8*I3xx*cos(2*th2-2*thl+2*th3) 

+l/8*I3xx*cos(2*th2+2*thl+2*th3)-l/8*I3yy*cos(2*th2-2*thl+2*th3) 

-l/8*I3yy*cos(2*th2+2*thl+2*th3)-l/2*I3zz*cos(2*thl)-3/4*m2*rc2A2*cos(2*th2) 

-l/8*m3*rc3A2*cos(2*th2-2*thl+2*th3)-l/8*m3*rc3A2*cos(2*th2+2*thl+2*th3) 

-l/4*m3*rc3*a2*cos(th3+2*th2+2*thl)-l/4*m3*rc3*a2*cos(th3+2*th2-2*thl) 

-3/2*m3*rc3*a2*cos(2*th2+th3)+2*m3*rc3*dl*sin(th3+th2)+2*m3*rc3*d0*sin(th3+th2) 

141100(2,3)=  -sin(thl)*cos(thl)*(-I3xx-I2yy+m3*rc3A2+Ilzz-m3*rc3A2*cos(th3)A2-Ilxx+I3zz+I2zz 

-  cos(th3)A2*I3yy+cos(th3)A2*I3xx+I2yy*cos(th2)A2+I3xx*cos(th2)A2+2*m3*rc3*cos(th3)*cos(th2)A2*a2 
-I3yy*cos(th2)A2-cos(th2)A2*I2xx-2*cos(th2)*sin(th3)*m3*rc3*sm(th2)*a2 
-2*cos(th2)*cos(th3)*I3yy*sin(th3)*sin(th2)+2*cos(th2)*sm(th3)*I3xx*cos(th3)*sin(th2) 
-2*cos(th2)*sin(th3)*m3*rc3A2*cos(th3)*sin(th2)+2*I3yy*cos(th3)A2*cos(th2)A2+m3*cos(th2)A2*a2A2 
-m3*rc3A2*cos(th2)A2-2*I3xx*cos(th3)A2*cos(th2)A2+m2*cos(th2)A2*rc2A2 
+2*m3*rc3A2*cos(th3)A2*cos(th2)A2) 

NRC,o(3,l)=  I2zz*cos(2*thl)+l/2*m3*rc3*a2*cos(th3-2*thl)+l/2*m3*rc3*a2*cos(th3+2*thl) 
+l/4*m3*rc3A2*cos(2*th2-2*M+2*th3)+l/4*m3*rc3A2*cos(2*th2+2*thl+2*th3) 
-l/4*I3xx*cos(2*th2-2*thl+2*th3)-l/4*I3xx*cos(2*th2+2*thl+2*th3) 
+l/2*m3*rc3*a2*cos(th3+2*th2-2*thl)+l/2*m3*rc3*a2*cos(th3+2*th2+2*thl) 

+1/4*12  yy*cos(-2*thl+2*th2)+l/4*I2yy*cos(2*thl+2*th2)+Ilzz*cos(2*thl) 

+l/4*m2*rc2A2*cos(-2*thl+2*th2)+l/4*m2*rc2A2*cos(2*thl+2*th2)+l/2*m2*rc2A2*cos(2*thl) 

-l/2*I2yy*cos(2*thl)+l/2*m3*rc3A2*cos(2*thl)-l/4*I2xx*cos(-2*thl+2*th2) 

-l/4*I2xx*cos(2*thl+2*th2)-l/2*I2xx*cos(2*thl)-Ilxx*cos(2*thl) 

+l/4*m3*a2A2*cos(-2*thl+2*th2)+l/4*m3*a2A2*cos(2*thl+2*th2)+l/2*m3*a2A2*cos(2*thl) 

-l/2*I3xx*cos(2*thl)+I3zz*cos(2*thl)+I0yy-I0xx-l/2*I3yy*cos(2*thl) 

+1/4*13  yy*cos(2*th2+2*thl+2*th3)+l/4*I3yy*cos(2*th2-2*thl+2*th3) 

NRCt0(3,2)=  -  sin(thl)*(2*cos(th3)*m3*rc3A2*sin(th3)*cos(th2)A2+cos(th2)*rc2*m2*d0 
+2*sin(th2)*I3yy*cos(th3)A2*cos(th2)+cos(th2)*m2*sin(th2)*rc2A2-2*cos(th2)*I3xx*cos(th3)A2*sm(th2) 
+cos(th2)*m3*dl*a2-sin(th3)*m3*rc3*a2+2*sin(th3)*I3yy*cos(th3)*cos(th2)A2 
-sin(th2)*sin(th3)*m3*rc3*dl-sin(th2)*sin(th3)*m3*rc3*d0-cos(th2)*I2xx*sin(th2) 

-  sin(th3)*I3yy*cos(th3)+sin(th3)*I3xx*cos(th3)+2*cos(th2)*m3*rc3A2*cos(th3)A2*sm(th2) 
+2*cos(th2)*cos(th3)*m3*rc3*sin(th2)*a2+2*m3*sin(th3)*cos(th2)A2*rc3*a2+cos(th2)*cos(th3)*m3*rc3 
*d0+cos(th2)*cos(th3)*m3*rc3*dl-cos(th2)*m3*rc3A2*sm(th2)+cos(th2)*m3*d0*a2 
-2*cos(th3)*I3xx*sin(th3)*cos(th2)A2-sm(th3)*m3*rc3A2*cos(th3)+cos(th2)*m3*sin(th2)*a2A2 
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-sin(th2)*I3)^y*cos(th2)+cos(th2)*I3xx*sin(th2)+sin(tli2)*I2yy*cos(th2)+cos(th2)*rc2*in2*dl) 

NRCt0(353)=  -cos(thl)!ic(sin(th2)*m3*rc3A2*cos(th2)-cos(th2)*rc2*m2*d0 

-2*sin(th3)*I3yy*cos(th3)*cos(th2)A2+sin(th2)*sin(th3)*m3*rc3*dl-cos(th2),|cm3*d0*a2 

-2*sin(th2)*I3yy*cos(th3)A2*cos(th2)-cos(th2)*m3*sin(th2)*a2A2-  cos(th2)*m3*dl*a2 

+2*cos(th2)*I3xx*cos(th3)A2*sin(th2)+cos(th2)*I3yy*sin(1h2)+sin(th3)*I3yy*cos(th3) 

-sin(th2)*I2yy*cos(th2)+cos(th2)*I2xx*sin(th2)-cos(th2)*I3xx*sm(th2) 

-2*cos(th2)*cos(th3)*m3*rc3*sin(th2)*a2-cos(th2)*cos(th3)*m3*rc3*dO 

-2*cos(th2)*m3*rc3A2*cos(th3)A2*sin(th2)-cos(th2)*cos(th3)*m3*rc3*dl 

-cos(th3)*I3xx*sin(th3)+m3*sin(th3)*rc3*a2-cos(th2)*m2*sm(th2)*rc2A2-cos(th2)*rc2*m2*dl 

-2*cos(th3)*m3*rc3A2*sin(th3)*cos(th2)A2+sin(th2)*sin(th3)*m3*rc3*d0 

-2*m3*sin(th3)*cos(th2)A2*rc3*a2+2*cos(th3)*I3xx*sin(th3)*cos(th2)A2+sin(th3)*m3*rc3A2*cos(th3)) 

(A- 19) 


Flexible  Body  Centrifugal  Interaction  Torques 


NCCtO(1,1)=0 

NCCx0(l,2)=  sin(thl)*(-I3xx*sin(th2)*cos(th2)-cos(th2)*I2yy*sin(th2)-  I3xx*sin(th3)*cos(th3) 
+I3yy*sin(th3)*cos(th3)+sin(th2)*I2xx*cos(th2)+I3yy*sin(th2)*cos(th2) 

-2*m3*rc3A2*cos(th3)A2*cos(th2)*sin(th2)-m3*rc3*cos(th3)*cos(tl^)*dO-ni3*rc3*cos(th3)*cos(th2)*dl 
-2*m3*rc3*cos(th3)*cos(th2)*sin(th2)*a2+m3*rc3*sin(th3)*sin(th2)*d0 
•>2*m3*rc3*sin(th3)*a2*cos(th2)A2-2*m3*rc3A2*sin(th3)*cos(th3)*cos(th2)A2 
+m3*rc3*sin(th3)*sin(th2)*dl-m3*cos(th2)*sin(th2)*a2A2-  m3*cos(th2)*dl*a2 
+2*I3xx*cos(th3)A2*cos(th2)*sin(th2)-rc2*m2*cos(th2)*dl-2*I3yy*cos(th3)A2*cos(th2)*sin(th2) 
-m3*cos(th2)*dO*a2+m3*rc3A2*sin(th2)*cos(th2)-  m2*cos(th2)*sin(th2)*rc2A2 
+2*I3xx*cos(th3)*cos(th2)A2*sin(th3)-2*I3yy*sin(th3)*cos(th3)*cos(th2)A2 
~rc2*m2*cos(th2)*d0+m3*rc3A2*sin(th3)*cos(th3)+m3*rc3*sin(th3)*a2) 

NCCx0(l,3)=  sin(thl)*(cos(th2)*I2yy*sin(th2)-I3yy*sin(th3)*cos(th3)-  I3yy*sin(th2)*cos(th2) 
+I3xx*sin(th2)*cos(th2)+2*m3*rc3*cos(th3)*cos(th2)*sin(th2)*a2+2*m3*rc3*sin(th3)*a2*cos(th2)A2 
+m3*rc3*cos(th3)*cos(th2)*d0+2*m3*rc3A2*cos(th3)A2*cos(th2)*sin(th2)+m3*rc3*cos(th3)*cos(th2)*dl 
+2*m3*rc3A2*sin(th3)*cos(th3)*cos(th2)A2-2*I3xx*cos(th3)*cos(th2)A2*sin(th3) 

-  m3*rc3*sin(th3)*sin(th2)*d0-m3*rc3*sin(th3)*sin(th2)*dl-  sin(th2)*I2xx*cos(th2) 
+I3xx*sin(th3)*cos(th3)-m3*rc3*sin(th3)*a2-m3*rc3A2*sin(th3)*cos(th3)-  m3*rc3A2*sin(th2)*cos(th2) 
+m3*cos(th2)*sin(th2)*a2A2+m3*cos(th2)*d0*a2+m2*cos(th2)*sin(th2)*rc2A2 
-2*I3xx*cos(th3)A2*cos(th2)*sin(th2)  +2*I3yy*sin(th3)*cos(th3)*cos(th2)A2 

H-m3*cos(th2)*dl*a2+rc2*m2*cos(th2)*d0+rc2*m2*cos(th2)*dl+2*I3yy*cos(th3)A2*cos(th2)*sm(th2)) 

NCCx0(2,l)=  cos(thl)*Csin(th3)*I3yy*cos(th3)+cos(th3)*I3xx*sin(th3)-sm(th2)*I3yy*cos(th2) 

-  sin(tb2)5|cI2xx*cos(th2)+cos(th2)*I2yy*sin(th2)+cos(th2)*I3xx*sin(th2) 
+2*sin(th3)*m3>,crc3A2*cos(th3)*cos(th2)A2-  m3*rc3*sin(th3)*sin(th2)*d0 
+2*m3*sin(th3)*rc3*a2*cos(th2)A2+2*sin(th2)*xn3*rc3A2*cos(th3)A2*cos(th2) 

-m3*sin(th3)*sin(th2)*rc3*dl+2*cos(th3)*I3yy*sin(th3)*cos(th2)A2-2*sin(th2)*I3xx*cos(th3)A2*cos(th2) 

+sin(th2)*m2*cos(th2)*rc2A2+m3*rc3*cos(th3)*cos(th2)*d0+m3*cos(th3)*cos(th2)*rc3*dl 

+m2*cos(th2)*rc2*dl+2*sin(th2)*cos(th3)*m3*rc3*cos(th2)*a2+m3*cos(th2)*d0*a2 

+2*cos(th2)*I3yy*cos(th3)A2*sin(th2)-cos(th2)*m3*rc3A2*sin(th2)-2*cos(th3)*I3xx*sin(th3)*cos(th2)A2 

-cos(th3)*m3*rc3A2*sin(th3)-  m3*sin(th3)*rc3*a2+sin(th2)*m3*cos(th2)*a2A2 

+m3  *cos(th2)*a2  *d  1  +rc2  *m2  *cos(th2)*d0) 


NCCx0(2,2)=0 
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NCCt0(2,3)=  -cos(thl)*(-sin(th2)*I3yy*cos(th2)+cos(th3)*I3xx*sin(th3)-  sin(th2)*I2xx*cos(th2) 
+cos(th2)*I3xx*sin(th2)-  sin(th3)*I3yy*cos(th3)+2*sin(th2)*m3*rc3A2*cos(th3)A2*cos(th2) 
+2*m3*sin(th3)*rc3*a2*cos(th2)A2-  ni3*sin(th3)*sin(th2)*rc3*dl+m3*rc3*cos(th3)*cos(th2)*dO 
+2*sin(th3)*m3*rc3A2*cos(th3)*cos(th2)A2+m3*cos(th3)’,ccos(th2)*rc3*dl-  m3*rc3*sin(th3)*sin(th2)*dO 
+2*sin(th2)*cos(th3)*m3*rc3*cos(th2)*a2-cos(th3)*m3*rc3A2*sin(th3)-  cos(th2)*m3*rc3A2*sin(th2) 
+rc2*m2*cos(th2)*d0+sin(th2)*m3*cos(th2)*a2A2+m3*cos(th2)*a2*dl+sin(th2)*m2*cos(th2)*rc2A2 
-2*sin(th2)*I3xx*cos(th3)A2*cos(th2)+m2*cos(th2)*rc2*dl+m3*cos(th2)*d0*a2 
+2*cos(th3)*I3yy*sin(th3)*cos(th2)A2-m3*sin(th3)*rc3*a2+2*cos(th2)*I3yy*cos(th3)A2*sin(th2) 
-2*cos(th3)*I3xx*sin(th3)*cos(th2)A2+cos(th2)*I2yy*sin(th2)) 


NCCt0(3j1):=  -sin(thl)*cos(thl)*(I3zz+I2zz+rc2A2*cos(th2)A2*m2+a2A2*cos(th2)A2*m3 

-2*cos(th2)A2*cos(th3)A2*I3xx-cos(th3)A2*rc3A2*m3-  cos(th2)A2*rc3A2*m3 

+2*cos(th3)A2*cos(th2)A2*I3yy-I3xx-I2yy+rc3A2*m3-cos(th3)A2*I3yy-  cos(th2)A2*I3yy 

+2*cos(th2)A2*cos(th3)A2*rc3A2*m3+cos(th2)A2*I3xx+2*cos(th2)*cos(th3)*I3xx*sin(th3)*sin(th2) 

-2*cos(th2)*cos(th3)*rc3A2*m3*sin(th3)*sin(th2)+2*a2*cos(th2)A2*rc3*m3*cos(th3) 

-2*a2*cos(th2)*rc3*m3*sin(th3)*sin(th2)-2*cos(th2)*sin(th3)*I3yy*cos(th3)*sm(th2) 

-cos(th2)A2*I2xx+cos(th2)A2*I2yy+cos(th3)A2*I3xx-Ilxx+Ilzz) 


NCCt0(3,2)=  sin(thl)*cos(thl)*(I3zz+I2zz-I3xx-I2yy+rc3A2*m3-Ilxx+Ilzz-  cos(th2)A2*I2xx 
+cos(th2)A2*I2yy+cos(th2)A2*I3xx-  cos(th2)A2*I3yy+cos(th3)A2*I3xx 

+2*a2*cos(th2)A2*rc3*m3*cos(th3)-2*cos(di2)A2*cos(th3)A2*I3xx+2*cos(th2)A2*cos(th3)A2*rc3A2*m3 
-  cos(th3)A2*rc3A2*m3+a2A2*cos(th2)A2*m3+2*cos(th3)A2*cos(th2)A2*I3yy+rc2A2*cos(th2)A2*m2 
+2*cos(th2)*cos(th3)*I3xx*sin(th3)*sin(th2)“2*cos(th2)*sin(th3)*I3yy*cos(th3)*sin(th2) 
-2*cos(th2)*cos(th3)*rc3A2*m3*sin(th3)*sin(th2)-cos(th3)A2*I3yy 
-2*a2*cos(th2)*rc3*m3*sin(th3)*sin(th2)-cos(th2)A2*rc3A2*m3) 


NCCt0(3,3)=0 


(A-20) 


Flexible  Body  Rotational  Inertia  Interaction  Torques 

Bw0(l,l)“  2*rc2*m2*sin(th2)*dl+2*m3*rc3*cos(th3)*a2-  m3*rc3A2*cos(thl)A2 

+I3yy*cos(th3)A2*cos(thl)A2-I3xx*cos(thl)A2*cos(th2)A2-2*I3yy*cos(th2)A2*cos(thl)A2*cos(th3)A2 

-I2zz*cos(thl)A2+m3*d0A2+I3xx*cos(thl)A2+2*I3xx*cos(th3)A2*cos(th2)A2*cos(thl)A2 

-Ilzz*cos(thl)A2+Ilzz+2*rc2’,cm2*sin(th2)*d0+m3*rc3A2+Ilxx*cos(thl)A2+2*m3*sin(th2)*d0*a2 

-  I2yy*cos(thl)A2*cos(th2)A2+I3yy*cos(th2)A2*cos(thl)A2+2*m3*sin(th2)*dl*a2 

+2*cos(th2)*sin(th3)*m3*rc3*sin(th2)*cos(thl)A2*a2- 

2*cos(th2)*I3xx*cos(th3)*sin(th3)*sin(th2)*cos(thl)A2-2*m3*rc3*cos(th3)*a2*cos(thl)A2*cos(th2)A2 

+2*cos(th2)*sin(th3)*m3*rc3A2*cos(th3)*sin(th2)*cos(thl)A2 

+2*cos(th2)*sin(th3)*I3yy*cos(th3)*sin(th2)*cos(thl)A2+2*m3*rc3*cos(th3)*sin(th2)*d0 

+2*m3*rc3*sin(th3)*cos(th2)*d0-  I3xx*cos(thl)A2*cos(th3)A2+I2xx*cos(th2)A2*cos(thl)A2 

+2*rcl*ml*d0+23,<ni3*d0*dl+2*m2*d0*dl-ni3*cos(thl)A2s,ca2A2*cos(th2)A2+m3*a2A2 

-2*m3*rc3A2*cos(th2)A2*cos(thl)A2*cos(th3)A2+2*m3*rc3*cos(th3)*sin(th2)*dl 

+2*m3*rc3*sin(th3)*cos(th2)*dl+I2zz+I3zz+I2yy*cos(thl)A2+m3*dlA2+ml*d0A2 

-I3zz*cos(thl)A2+m3*rc3A2*cos(th2)A2*cos(thl)A2+I0xx+m0*rc0A2+m2*rc2A2+m2*d0A2+ml*rclA2 

+m2*dl  A2-m2*rc2A2*cos(thl  )A2*cos(th2)A2+m3  *rc3  A2*cos(th3)A2*cos(thl  )A2 


Bwo(  1 ,2)-  -cos(thl  )*sin(thl  )*(m3  *rc3  A2-I3xx-I2yy-m3  *rc3  A2*cos(th2)  A2+m3  *a2  A2 *cos(th2)  A2 
-2*I3xx*cos(th3)A2*cos(th2)A2+2*I3yy*cos(th3)A2*cos(th2)A2 

-2*cos(th2)*sin(th3)*m3*rc3A2*cos(th3)*sin(th2)+2*cos(th2)*I3xx*cos(th3)*sin(th3)*sin(th2) 
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+2*m3*rc3*cos(th3)*a2*cos(th2)A2-2*cos(th2)*sin(th3)*I3yy*cos(th3)*sm(th2) 

-cos(th3)A2*rc3A2*m3+m2*rc2A2*cos(th2)A2-2*cos(th2)*sin(th3)*m3*rc3*sin(th2)*a2+cos(th3)A2*I3xx 

-cos(th3)A2*I3yy-I3yy*cos(th2)A2+I3xx*cos(th2)A2+I2yy*cos(th2)A2 

-I2xx*cos(th2)A2+2*m3*rc3A2*cos(th2)A2*cos(th3)A2+Ilzz-Ilxx+I2zz+I3zz) 

Bw0(l,3)=-  cos(thl)*(m3*cos(th3)*cos(th2)*rc3*d0+sin(th2)*m3*cos(th2)*a2A2 

+sin(th2)*m2*cos(th2)*rc2A2+m2*cos(th2)*rc2*dl-cos(th2)*I2xx*sm(th2) 

+2*sin(th2)*cos(th3)*m3*rc3*cos(th2)*a2-m3*sin(th3)*sin(th2)*rc3*dl-cos(th2)*in3*rc3A2*sin(th2) 

-cos(th3)*I3yy*sin(th3)-a2*rc3*sm(th3)*m3-  sin(th3)*rc3A2*m3*cos(th3)+m2*cos(th2)*rc2*d0 

+m3  *cos(th2)  *a2*d 1  +sm(th2)*I2yy*cos(th2)+m3  *cos(th2)*a2*d0-2  *cos(th2)*I3xx*cos(th3)  A2  *sin(th2) 

+sm(th3)*cos(th3)*I3xx+sin(th2)*I3xx*cos(th2)+2*sin(th3)*m3*rc3A2*cos(th3)*cos(th2)A2 

-2*I3xx*cos(th3)*sin(th3)*cos(th2)A2+2*sin(th2)*I3yy*cos(th3)A2*cos(th2) 

+2*sin(th3)*I3yy*cos(th3)*cos(th2)A2+m3*cos(th3)*cos(th2)*rc3*dl-  m3*sin(th3)*sin(th2)*rc3*dO 

+2*sin(th3)*m3*rc3*cos(th2)A2*a2+2*sin(th2)*m3*rc3A2*cos(th3)A2*cos(th2)-cos(th2)*I3yy*sm(th2)) 

Bw0(2,l)=  -sin(thl)*cos(thl)*(m3*rc3A2-I3xx-I2yy-2*cos(th2)*sin(th3)*m3*rc3*sin(th2)*a2 

-2*cos(th2)*sin(th3)*m3*rc3A2*cos(th3)*sin(th2)+2*m3*rc3*cos(th3)*a2*cos(th2)A2 

-2*cos(th2)*sin(th3)*I3yy*cos(th3)*sin(th2)+2*cos(th2)*I3xx*cos(th3)*sin(th3)*sin(th2) 

+2*m3*rc3A2*cos(th2)A2*cos(th3)A2+I2yy*cos(th2)A2-I2xx*cos(th2)A2+I3xx*cos(th2)A2 

-I3yy*cos(th2)A2+cos(th3)A2*I3xx-cos(th3)A2*I3yy+2*I3yy*cos(th3)A2*cos(th2)A2+m3*a2A2*cos(th2)A2 

-m3*rc3A2*cos(th2)A2-cos(th3)A2*rc3A2*m3+m2*rc2A2*cos(th2)A2-2*I3xx*cos(th3)A2*cos(th2)A2 

-1 1  xx+1 1  zz+I2zz+I3zz) 

Bw0(2,2)=  !4*I2xx*cos(2*th2)-  !4*I2yy*cos(2*th2)+l/4*I3xx+l/4*I2yy+l/4*I3yy+l/4*I2xx 
+l/4*I3xx*cos(2*th3+2*th2)-l/4*I3yy*cos(2*th3+2*th2)+2*m3*rc3*dl*sin(th3+th2) 

- 1/4  *m3  *rc3  A2*cos(2*th3+2*th2)+3/4*m3  *rc3A2+2*rc  1  *ml  *d0+m3  *dl  A2+m2*dl  A2 

+3/4*m2*rc2A2+m3*d0A2+m2*d0A2+l/2*Ilxx+l/2*Ilzz+l/2*I2zz+l/2*I3zz+3/2*cos(th3)*m3*rc3*a2 

+2*m3*d0*dl+2*m2*d0*dl+2*sin(th2)*m3*d0*a2+2*sm(th2)*m3*dl*a2+2*sin(th2)*rc2*in2*dl 

+ml*d0A2+ml*rclA2+l/4*m3*rc3*a2*cos(th3+2*thl)+2*m3*rc3*d0*sm(th3+th2)+I0yy 

+l/4*m3*rc3*a2*cos(th3+2*th2-2*thl)+l/4*m3*rc3*a2*cos(th3+2*th2+2*thl) 

+l/4*m3*rc3*a2*cos(th3-2*thl)+m0*rc0A2+3/4*m3*a2A2-l/4*m3*a2A2*cos(2*th2) 

-l/2*m3*rc3*a2*cos(2*th2+th3)-l/4*m2*rc2A2*cos(2*th2)-  ‘/4*I3xx*cos(2*thl) 

+l/4*m3*rc3A2*cos(2*thl)-l/4*I3yy*cos(2*thl)-l/4*I2yy*cos(2*thl) 

+l/8*I2yy*cos(-2*thl+2*th2)+l/8*I2yy*cos(2*thl+2*th2)-l/4*I2xx*cos(2*thl)+2*sin(th2)*rc2*m2*d0 
-l/8*I2xx*cos(-2*thl+2*th2)-  l/8*I2xx*cos(2*thl+2*th2)+l/4*m2*rc2A2*cos(2*thl) 
+l/8*m2*rc2A2*cos(-2*thl+2*th2)+l/8*m2*rc2A2*cos(2*thl+2*th2)  +l/4*m3*a2A2*cos(2*thl) 
+l/8*m3*a2A2*cos(-2*thl+2*th2)+l/8*m3*a2A2*cos(2*thl+2*th2) 
+l/8*m3*rc3A2*cos(2*th2-2*thl+2*th3)+l/8*m3*rc3A2*cos(2*th2+2*thl+2*th3) 

+1/8*13  yy*cos(2*th2-2*thl+2*th3)+l/8*I3yy*cos(2*th2+2*thl+2*th3) 

-l/8*I3xx*cos(2*th2-2*thl+2*th3)-l/8*I3xx*cos(2*th2+2*thl+2*th3)-1/2*Ilxx*cos(2*thl) 

+l/2*I3zz*cos(2*thl)+l/2*Ilzz*cos(2*thl)+l/2*I2zz*cos(2*thl) 


Bw0(2,3)=  sm(thl)*(cos(th2)*I3yy*sin(th2)-I3xx*sin(th3)*cos(th3)-sin(th2)*I3xx*cos(th2) 

-sin(th2)*I2yy*cos(th2)+2*cos(th2)*I3xx*cos(th3)A2*sin(th2)-m3*cos(th2)*a2*dl 

-m3  *cos(th3)*cos(th2)*rc3  *dO+m3  *sin(th3)  *rc3  *a2-sin(th2)*m3  *cos(th2)  *a2  A2 

-2*sin(th3)*m3*rc3A2*cos(th3)*cos(th2)A2+cos(th3)*I3yy*sin(th3)-m3*cos(th2)*a2*d0 

-2*sin(th2)*m3*rc3A2*cos(th3)A2*cos(th2)-2*sin(th2)*cos(th3)*m3*rc3*cos(th2)*a2 

-2*sin(th2)*I3yy*cos(th3)A2*cos(th2)-m3*cos(th3)*cos(th2)*rc3*dl-m2*cos(th2)*rc2*dl 

-m2*cos(th2)*rc2*d0+2*I3xx*cos(th3)*sin(th3)*cos(th2)A2-2*sin(th3)*I3yy*cos(th3)*cos(th2)A2 

+m3  *sin(th3)*sin(th2)*rc3  *dO+cos(th3)*m3  *rc3A2*sin(th3)+m3  *sin(th3)*sin(th2)*rc3  *dl 

+cos(th2)*I2xx*sin(th2)-2*sm(th3)*m3*rc3*cos(th2)A2*a2+cos(th2)*m3*rc3A2*sin(th2) 

-sin(th2)*m2*cos(th2)*rc2A2) 
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Bw0(3,l)=  -cos(thl)*(cos(th2)*rc2*m2*d0+cos(th2)*m3*d0*a2-sin(th3)*m3*rc3A2*cos(th3) 

-Sm(th3)*m3*rc3*a2+cos(th2)*cos(th3)*m3*rc3*dl-sin(th2)*sin(th3)*rn3*rc3*d0 

-  sm(th2)*sin(th3)*m3*rc3*dl+2*cos(th2)*cos(th3)*m3*rc3*sm(th2)*a2 

+cos(th2)*cos(th3)*m3*rc3*d0+2*cos(th2)*I3yy*cos(th3)A2*sin(th2)+cos(th2)*rc2*m2*dl 

+2*m3*sin(th3)*cos(th2)A2*rc3*a2+2*cos(th2)*m3*rc3A2*cos(th3)A2*sin(th2) 

+2*cos(th3)*m3*rc3A2*sin(th3)*cos(th2)A2+2*cos(th3)*I3yy*sin(th3)*cos(th2)A2 

-2*sin(th2)*I3xx*cos(th3)A2*cos(th2)+cos(th2)*m3*dl*a2-2*I3xx*sm(th3)*cos(th3)*cos(th2)A2 

-sin(th2)*m3*rc3A2*cos(th2)+cos(th2)*m2*sin(th2)*rc2A2+cos(th2)*m3*sin(th2)*a2A2 

-sin(th2)*I3yy*cos(th2)+I3xx*cos(th3)*sin(th3)-  sin(th3)*I3yy*cos(th3)+cos(th2)*I3xx*sin(th2) 

+cos(th2)*I2yy*sin(th2)-sin(th2)*I2xx*cos(th2)) 

Bw0(3,2)=  -sin(thl)*(I3xx*sin(th3)*cos(th3)  -cos(th2)*I2xx*sin(th2)+sin(th2)*I3xx*cos(th2) 

+sm(th2)*I2yy*cos(th2)-cos(th2)*I3yy*sin(th2)-cos(th3)*I3yy*sin(th3)-m3*sin(th3)*sin(th2)*rc3*dl 

-m3*sm(th3)*sin(th2)*rc3*d0+2*sin(th3)*m3*rc3*cos(th2)A2*a2-  cos(th3)*m3*rc3A2*sin(th3) 

+2*sin(th3)*I3yy*cos(th3)*cos(th2)A2+2*sin(th3)*m3*rc3A2*cos(th3)*cos(th2)A2+m2*cos(th2)*rc2*dl 

+m2*cos(th2)*rc2*d0+m3*cos(th3)*cos(th2)*rc3*d0+m3*cos(th2)*a2*d0+m3*cos(th2)*a2*dl 

-rn3*sin(th3)*rc3*a2-2*cos(th2)*I3xx*cos(th3)A2*sin(th2)+sin(th2)*m3*cos(th2)*a2A2 

+sin(th2)*m2*cos(th2)*rc2A2+2*sin(th2)*cos(th3)*m3*rc3*cos(th2)*a2-  cos(th2)*m3*rc3A2*sin(th2) 

+m3*cos(th3)*cos(th2)*rc3*dl+2*sm(th2)*m3*rc3A2*cos(th3)A2*cos(th2)+2*sin(th2)*I3yy*cos(th3)A2*c 

os(th2)-2*I3xx*cos(th3)*sin(th3)*cos(th2)A2) 

Bw0(3,3)=  Ilyy+I0zz+l/2*m3*rc3A2-l/2*I3xx*cos(2*th3+2*th2)+l/2*I3xx+l/2*I2yy 

+1/2*13  yy+l/2*I2xx+cos(th3)*m3*rc3*a2+l/2*I2yy*cos(2*th2)+l/2*m3*a2A2+l/2*m3*a2A2*cos(2*th2) 

+l/2*I3yy*cos(2*th3+2*th2)+l/2*m2*rc2A2- 

l/2*I2xx*cos(2*th2)+m3*rc3*a2*cos(2*th2+th3)+l/2*m2*rc2A2*cos(2*th2)+l/2*m3*rc3A2*cos(2*th3+2 
*th2)  (A-21) 


Cross  Coupling  Interaction  Torque  Effects 

NCMT0(l,l)=2*sm(thl)*cos(thl)*(I3zz+I2zz+cos(th2)A2*I2yy+cos(th3)A2*I3xx-cos(th2)A2*I2xx-I3xx 

-I2yy+cos(th2)A2*I3xx-cos(th2)A2*I3yy+rc3A2*m3+2*cos(th2)A2*cos(th3)A2*rc3A2*m3 

-2*cos(th2)*sin(th3)*I3yy*cos(th3)*sin(th2)+2*cos(th3)A2*cos(th2)A2*I3yy 

-2*cos(th2)A2*cos(th3)A2*I3xx+a2A2*cos(th2)A2*m3+rc2A2*cos(th2)A2*m2-cos(th2)A2*rc3A2*m3 

-cos(th3)A2*I3yy+2*cos(th2)*cos(th3)*I3xx*sin(th3)*sin(th2)-  2*a2*cos(th2)*rc3*m3*sin(th3)*sin(th2) 

+2*a2*cos(th2)A2*rc3*m3*cos(th3)-2*cos(th2)*cos(th3)*rc3A2*m3*sin(th3)*sin(th2) 

-cos(th3 )  A2  *rc3  A2  *m3  -1 1  xx+1 1  zz) 

NCMl0(l,2)=  -1/2*13  xx*cos(2*th3+2*th2)-I2zz*cos(2*thl)+Ilxx*cos(2*thl) 

+l/4*I3xx*cos(2*th2-2*thl+2*th3)+l/4*I3xx*cos(2*th2+2*thl+2*th3)-l/2*a2*rc3*m3*cos(th3-2*thl) 

-l/2*a2*rc3*m3*cos(th3+2*thl)-l/4*rc2A2*m2*cos(-2*thl+2*th2)-l/4*rc2A2*m2*cos(2*thl+2*th2) 

-l/2*rc2A2*m2*cos(2*thl)+l/2*I3yy*cos(2*th3+2*th2)-l/4*a2A2*m3*cos(-2*thl+2*th2) 

-l/2*a2A2*m3*cos(2*thl)+l/2*I2yy*cos(2*thl)+l/2*I3xx+l/2*I2yy+l/2*I3yy+l/2*I2xx+l/2*a2A2*m3 

+l/2*a2A2*m3*cos(2*th2)+a2*rc3*m3*cos(th3)+l/2*rc3A2*m3+l/2*rc2A2*m2 

+l/2*rc2A2*m2*cos(2*th2)-l/4*a2A2*m3*cos(2*thl+2*th2)+a2*rc3*m3*cos(2*th2+th3) 

-l/4*I2yy*cos(2*thl+2*th2)-l/4*I2yy*cos(-2*thl+2*th2)+l/4*I2xx*cos(2*thl+2*th2) 

+l/4*I2xx*cos(-2*thl+2*th2)+l/2*I3xx*cos(2*thl)+l/2*I2xx*cos(2*thl)-Ilzz*cos(2*thl) 

-l/4*rc3A2*m3*cos(2*th2-2*thl+2*th3)-l/4*rc3A2*m3*cos(2*th2+2*thl+2*th3) 

-l/2*rc3A2*m3*cos(2*thl)+Ilyy+l/2*I3yy*cos(2*thl)+l/2*I2yy*cos(2*th2) 

-l/2*a2*rc3*m3*cos(th3+2*th2-2*thl)-l/2*a2*rc3*m3*cos(th3+2*th2+2*thl) 

-l/4*I3yy*cos(2*th2+2*thl+2*th3)-l/4*I3yy*cos(2*th2-2*thl+2*th3)-l/2*I2xx*cos(2*th2) 
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-I3zz*cos(2*thl)+l/2*rc3A2*m3*cos(2*th3+2*th2) 


NCMto(1,3)=  4/2*I3yy*cos(2*th3+2*th2+thl)+l/2*I3yy*cos(2*th3+2*th2-thl) 

-l/2*I3xx*cos(2*th3+2*th2-thl)+l/2*I3xx*cos(2*th3+2*th2+thl)-m3*rc3*dl*sin(-thl+th3+th2) 

+m3*rc3*dl*sin(thH-th3+th2)+l/2*rc3A2*m3*cos(2*th3+2*th2-thl) 

-  l/2*rc3A2*m3*cos(2*th3+2*th2+thl)  -l/2*m2*rc2A2*cos(2*th2+thl)+l/2*m2*rc2A2*cos(2*th2-thl) 
+m3*rc3*a2*cos(2*th2+th3-thl)-m3*rc3*a2*cos(2*th2+th3+thl)-l/2*m3*a2A2*cos(2*th2+thl) 
+l/2*m3*a2A2*cos(2*th2-thl)+l/2*I2xx*cos(2*th2+thl)-l/2*I2xx*cos(2*th2-thl) 
-l/2*I2yy*cos(2*th2+thl)+l/2*I2yy*cos(2*th2-thl)-m3*rc3*dO*sin(-thH-th3+th2) 
+m3*rc3*d0*sin(thH-th3+th2)+m3*a2*d0*sin(th2+thl)-m3*a2*d0*sin(th2-  thl) 
+m2*rc2*dl*sin(th2+thl)-m2*rc2*dl*sin(th2-thl)+m2*rc2*d0’!<sin(th2+thl)-m2*rc2*d0*sin(th2-thl) 
-m3  *a2*d  1  *sin(th2-thl  )+m3  *a2*dl  *sin(th2+th  1 ) 

NCMx0(l,4)=  l/2*I3yy*sin(2*th3+2*th2)+l/4*I3yy*sin(2*th2-2*thl+2*th3) 

+l/4*I3yy*sm(2*th2+2*thl+2*th3)-l/2*I2xx*sin(2*th2)-l/4*I2xx*sin(2*thl+2*tli2) 

4/4*I2xx*sin(-2*thl+2*th2)+2*m3*rc3M0*cos(th3+th2)+l/4*I2yy*sin(2*thl+2*th2) 

+2*m3*rc3*dl*cos(th3+th2)+l/4*m2*rc2A2*sin(2*thl+2*th2)+l/4*m2*rc2A2*sin(-2*thl+2*th2) 

+l/2*m2*rc2A2*sin(2*th2)+m3*rc3*a2*sin(2*th2+th3)+l/4*I2yy*sin(-2*thl+2*th2) 

H-l/2*I2yy*sin(2*th2)+l/2*m3*rc3*a2*sin(th3+2*th2-2*thl)+l/2*m3*rc3*a2*sin(th3+2*th2+2*thl) 

+l/2*m3*rc3A2*sin(2*th3+2*th2)+l/4*m3*rc3A2*sin(2*th2+2*thl+2*th3) 

+l/4*m3*rc3A2*sin(2*th2-2*thl+2*th3)+l/4*m3*a2A2*sin(2*thl+2*th2) 

+l/4*m3*a2A2*sin(-2*thl+2*th2)+l/2*m3*a2A2*sin(2*th2)-l/4*I3xx*sin(2*th2-2*thl+2*th3) 

-l/4*I3xx*sin(2*th2+2*thl+2*th3)-  l/2*I3xx*sin(2*th3+2*th2)+2*m2*cos(th2)*rc2*d0 

+2*m3*cos(th2)*a2*d0+2*m3*cos(th2)*a2*dl+2*m2’i<cos(th2)*rc2*dl 

NCMt0(1,5)=  -l/4*I2xx*cos(-2*thl+2*th2)+l/4*m2*rc2A2*cos(-2*thl+2*th2) 
-l/4*m3*a2A2*cos(2*thl+2*th2)+l/4*m3*a2A2*cos(-2*thl+2*th2)-  l/4*m2*rc2A2*cos(2*thl+2*th2) 
+l/4*I2xx*cos(2*thl+2*th2)M/2*m3*rc3*a2*cos(th3+2*th2-2*thl) 

-  l/2*m3*rc3*a2*cos(th3+2*th2+2*thl)-l/4*I2yy*cos(2*thl+2*th2)+l/4*I2yy*cos(-2*thl+2*th2) 
-l/4*I3yy*cos(2*th2+2*thl+2*th3)+l/4*I3yy*cos(2*th2-2*thl+2*th3) 
+l/4*I3xx*cos(2*th2+2*thl+2*th3)-l/4*I3xx*cos(2*th2-2*thl+2*th3) 
-l/4*m3*rc3A2*cos(2*th2+2*thl+2*th3)+l/4*m3*rc3A2*cos(2*th2-2*thl+2*th3) 


NCMx0(l,6)=  -cos(thl)*(-2*m2*rc2A2-I3xx-I2yy+I3yy+I2xx-I3zz-  I2zz+2*m2*rc2A2*cos(th2)A2 
+4*I3yy*cos(th3)A2*cos(th2)A2-4*I3xx*cos(th3)A2*cos(th2)A2+2*m3*a2A2*cos(tii2)A2 
-2*m3*rc3A2*cos(th2)A2-2*cos(th3)A2*rc3A2*m3-4*cos(th2)*sin(th3)*m3*rc3A2*cos(th3)*sin(th2) 
-4*cos(th2)*sin(th3)*m3*rc3*sm(th2)*a2-2*m3*cos(th3)*sin(th2)*rc3*dl 

-4*cos(th2)*cos(th3)*I3yys!£sm(th3)*sin(th2)-2*m3*sin(th3)*cos(th2)*rc3*dl-2*m2*sin(th2)*rc2*dl 
-2*m3*sin(th2)*a2*dl-4*a2*rc3*m3*cos(th3)+2*I2yy*cos(th2)A2 
+4*cos(th2)*cos(th3)*I3xx*sin(th3)*sin(th2)  -2*m2*sin(th2)*rc2M0-2*m3*sin(th2)*a2*d0 
+4*m3*rc3A2*cos(th2)A2*cos(th3)A2+4*cos(th3)*m3*rc3*a2*cos(th2)A2 

-2*m3*cos(th3)*sin(th2)*rc3*d0-2*m3*sin(th3)*cos(th2)*rc3*d0-2*I2xx*cos(th2)A2+2*cos(th3)A2*I3xx 

-2*cos(th3)A2*I3yy+2*I3xx*cos(th2)A2-2*I3yy*cos(th2)A2-2*m3*a2A2) 

NCMt0(1,7)=  l/2*I3yy*sin(2*th3+2*th2)+l/4*I3yy*sin(2*th2-2*thl+2*th3) 

+l/4*I3yy*sin(2*th2+2*thl+2*th3)-3/2*sin(th3)*m3*rc3*a2-l/2*I3xx*sin(2*th3+2*th2) 

+l/4*m3*rc3*a2*sin(th3+2*thl)+l/4*m3*rc3*a2*sin(th3-2*thl)-l/4*I3xx*sin(2*th2+2*thl+2*th3) 

-l/4*I3xx*sin(2*th2-2*thl+2*th3)+l/2*m3*rc3A2*sin(2*th3+2*th2) 

+l/4*m3*rc3A2*sin(2*th2+2*thl+2*th3)+l/4*m3*rc3A2*sin(2*th2-2*thl+2*th3) 

+l/2*m3*rc3*a2*sin(2*th2+th3)+l/4*m3*rc3*a2*sin(th3+2*th2-2*thl) 

+l/4*m3*rc3*a2*sin(th3+2*th2+2*thl)+2*m3*rc3*d0*cos(th3+th2)+2*m3*rc3*dl*cos(th3+th2) 
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NCMto(l,8)=  -l/4*m3*rc3*a2*cos(th3+2*thl)+l/4*m3*rc3*a2*cos(th3-2*thl) 
4/4*m3*rc3A2*cos(2*th2+2*thl+2*th3)+l/4*m3*rc3A2*cos(2*th2-2*thl+2*th3) 

+1/4*13  xx*cos(2*th2+2*thl+2*th3)-l/4*I3xx*cos(2*th2~2*thl+2*th3) 

4/4*m3*rc3*a2*cos(th3+2*th2+2*thl)+l/4*I3yy*cos(2*th2~2*thl+2*th3) 

+l/4*m3*rc3*a2*cos(th3+2*th2-2*thl)-l/4*I3yy*cos(2*th2+2*thl+2*th3) 

NCMx0(l,9)=  l/2*I3xx*cos(2*th3+2*th2+thl)+l/2*I3xx*cos(2*th3+2*th2-thl) 
-l/2*m3*rc3A2*cos(2*th3+2*th2-thl)-  l/2*m3*rc3A2*cos(2*th3+2*th2+thl) 
+m3*rc3*d0*sin(thl+th3+th2)+m3*rc3*d0*sin(-thl+th3+th2)-l/2*m3*rc3*a2*cos(2*th2+th3+thl) 
-l/2*m3*rc3*a2*cos(2*th2+th3-thl)+m3*rc3*dl*sin(-thl+th3+th2)+m3*rc3*dl*sin(thl+th3+th2) 
+cos(thl)*m3*rc3A24/2*I3yy*cos(2*th3+2*th2+thl)-l/2*I3yy*cos(2*th3+2*th2-  thl) 
+l/2*m3*rc3*a2*cos(th3-thl)+l/2*m3*rc3*a2*cos(th3+thl)+cos(thl)*I3zz 

NCMx0(2,l)=  -4*I3yy*cos(th3)A2*cos(th2)A2*cos(thl)A241xx+2*Ilxx*cos(thl)A2+2*I2yy*cos(thl)A2 

-I3xx-I2yy-I3yy-I2xx~Ilyy+Ilzz-2*cos(th2)A2*cos(thl)A2*I2yy+2*cos(th2)A2*I2xx*cos(thl)A2 

-2*m3*rc3A2*cos(thl)A2+2*I3yy*cos(th2)A2*cos(thl)A2+2*I3xx*cos(thl)A2-2*cos(thl)A2*I3zz 

-2*Ilzz*cos(thl)A2-2*I2zz*cos(thl)A2-4*I3xx*cos(th3)*sin(th2)*sin(th3)*cos(th2)*cos(thl)A2 

-4*m3*rc3*cos(th3)*cos(th2)A2*cos(thl)A2*a2+4*m3*rc3*sin(th3)*sin(th2)*cos(thl)A2*a2*cos(th2) 

+4*m3*rc3A2*cos(th3)*sin(th2)*sin(th3)*cos(th2)*cos(tlil)A2 

-4*m3*rc3A2*cos(th3)A2*cos(th2)A2*cos(thl)A2+4*I3yy*cos(th3)*sin(th2)*sin(th3)*cos(th2)*cos(thl)A2 

+4*I3xx*cos(th3)A2*cos(th2)A2*cos(thl)A2-2*m2*cos(th2)A2*cos(thl)A2*rc2A2 

+2*m3*rc3A2*cos(thl)A2*cos(th2)A2+2*m3*rc3A2*cos(thl)A2*cos(th3)A2 

-2*m3*cos(thl)A2*a2A2*cos(th2)A2+2*I3yy*cos(th3)A2*cos(thl)A2-2*I3xx*cos(th3)A2*cos(thl)A2 

-2*I3xx*cos(th2)A2*cos(thl)A2+I3zz+I2zz 


NCMx0(2,2)=  2*sin(thl)*cos(thl)*(-m3*rc3A2“2*cos(th2)A2*cos(th3)A2*rc3A2*m3+Ilxx+I3xx+I2yy 

-Ilzz+cos(th3)A2*I3yy-cos(th3)A2*I3xx-I3zz-cos(th2)A2*I2yy-  I2zz+cos(th2)A2*I2xx 

+2*cos(th2)*a2*sm(th2)*sin(th3)*rc3*m3+2*cos(th2)*sin(th3)*sin(th2)*cos(th3)*rc3A2*m3 

-cos(th2)A2*I3xx-2*a2*cos(th2)A2*cos(th3)*rc3*m3-2*cos(th2)*sin(th3)*sin(th2)*cos(th3)*I3xx 

+2*cos(th2)*sin(th3)*sin(th2)*cos(th3)*I3yy+cos(th2)A2*I3yy-  a2A2*cos(th2)A2*m3 

+2*cos(th2)A2*cos(th3)A2*I3xx-rc2A2*cos(th2)A2*m2+cos(th2)A2*rc3A2*m3 

-2*cos(th2)A2*cos(th3)A2*I3yy+cos(th3)A2*rc3A2*m3) 

NCMx0(2,3)“  l/2*I2xx*sin(2*th2-thl)4/2*I2yy*sin(2*th2+thl)-l/2*m2*rc2A2*sin(2*th2-thl) 

-l/2*m2*rc2A2*sin(2*th2+thl)+l/2*I2xx*sin(2*th2+thl)-l/2*I2yy*sin(2*th2-thl) 

-Hi2*rc2*d0*cos(th2-thl)-m2*rc2*d0*cos(th2+thl)-m3*a2*d0*cos(th2-thl)-m3*a2*d0*cos(th2+thl) 

-m3*rc3*d0*cos(thl+th3+th2)-m3*rc3*d0*cos(-thl+th3+th2)-m3*a2*dl*cos(th2-thl) 

-m3*a2*dl*cos(th2+thl)-m3*rc3*dl*cos(-thl+th3+th2)-m3*rc3*dl*cos(thl+th3+th2) 

-l/2*m3*a2A2*sin(2*th2+thl)4/2*m3*a2A2*sin(2*th24hl)~rn3*rc3*a2*sin(2*th2+th3+thl) 

-m3*rc3*a2*sin(2*th2+th3-thl)-m2*rc2*dl*cos(th2-thl)-  m2*rc2*dl*cos(th2+thl) 

+l/2*I3xx*sin(2*th3+2*th2-thl)+l/2*I3xx*sin(2*th3+2*th2+thl)4/2*I3yy*sin(2*th3+2*th2-thl) 

-l/2*I3yy*sin(2*th3+2*th2+thl)-l/2*m3*rc3A2*sin(2*th3+2*th2-thl) 

-l/2*m3*rc3A2*sin(2*th3+2*th2+thl) 

NCMx0(2,4)=  -l/4*I3xx*cos(2*th2-2*thl+2*th3)+l/4*I3xx*cos(2*th2+2*thl+2*th3) 

4/4*m3*rc3A2*cos(2*th2+2*thl+2*th3)-l/4*I3yy*cos(2*th2+2*thl+2*th3) 

+l/4*I3yy*cos(2*th2-2*thl+2*th3)+l/4*ni3*rc3A2*cos(2*th2-2*thl+2*th3) 

-l/4*m2*rc2A2*cos(2*thl+2*th2)+l/4*m2*rc2A2*cos(-2*thl+2*th2) 

+l/2*m3*rc3*a2*cos(th3+2*th2-2*thl)-l/2*m3*rc3*a2*cos(th3+2*th2+2*thl) 

4/4*I2yy*cos(2*thl+2*th2)+l/4*I2yy*cos(-2*thl+2*th2)-  l/4*m3*a2A2*cos(2*thl+2*th2) 

+l/4*m3*a2A2*cos(-2*thl+2*th2)+l/4*I2xx*cos(2*thl+2*th2)-l/4*I2xx*cos(-2*thl+2*th2) 
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NCMt0(2,5)=  4/2*I3xx*sin(2*th3+2*th2)+l/4*I3xx*sin(2*th2+2*thl+2*th3) 

+l/4*I3xx*sin(2*th2-2*thl+2*th3)-l/4*I3yy*sin(2*th2-2*thl+2*th3)-l/4*I3yy*sin(2*th2+2*thl+2*th3) 

4/4*m3*rc3A2*sin(2*th2-2*thl+2*th3)4/4*m3*rc3A2*sin(2*th2+2*thl+2*th3)-  l/2*I2xx*sin(2*th2) 

+l/2*m3*rc3A2*sm(2*th3+2*th2)+l/2*I2yy*sin(2*th2)+l/2*I3yy*sin(2*th3+2*th2) 

4/4*m3*a2A2*sin(-2*thl+2*th2)+l/2*iri3*a2A2*sin(2*th2)4/4*rn3*a2A2*sin(2*thl+2*th2) 

-l/4*m2*rc2A2*sin(2*thl+2*th2)-l/4*m2*rc2A2*sin(-2*thl+2*th2)+l/2*m2*rc2A2*sin(2*tii2) 

+2*m3*rc3*dl*cos(th3+th2)+2*m3*rc3*d0*cos(th3+th2)+l/4*I2xx*sin(2*thl+2*th2) 

+l/4*I2xx*sin(-2*thl+2*th2)+m3*rc3*a2*sin(2*th2+th3)4/4*I2yy*sin(2*thl+2*th2) 

-l/4*I2yy*sin(-2*thl+2*th2)+2*m2*cos(th2)*rc2*dl+2*rri2*cos(th2)*rc2*d0 

>l/2*m3*rc3*a2*sin(th3+2*th2-2*thl)-  l/2*m3*rc3*a2*sin(th3+2nh2+2nhl)+2*m3*cos(th2)*a2*dl 

+2*m3*cos(th2)*a2*d0 

NCMx0(2,6)=  -sin(thl)*(-2*cos(th3)A2*rc3A2*m3+2*m2*rc2A2*cos(th2)A2-2*m3*rc3A2*cos(th2)A2 

-4*I3xx*cos(th3)A2*cos(th2)A2+4*I3yy*cos(th3)A2*cos(th2)A2+2*m3*a2A2*cos(th2)A2- 

2*a2A2*m3+I2xx-I3xx+I3yy-I2yy-I2zz-I3zz+2*cos(th3)A2*I3xx-2*cos(th3)A2*I3yy+2*I2yy*cos(th2)A2 

-2*I3yy*cos(th2)A2+2*I3xx*cos(th2)A2-2*I2xx*cos(th2)A2-2*m2*sin(th2)*rc2*dl-2*m2*rc2A2 

■'2*m3*sin(th2)*a2*dl-2*m3*sin(th3)*cos(th2)*rc3*d0-2*m3*cos(th3)*sin(th2)*rc3*d0 

-4*cos(th2)*sin(th3)*m3*rc3*sin(th2)*a2-2*m3*sin(th3)*cos(th2)*rc35,cdl 

+4*cos(th3)*m3*rc3*a2*cos(th2)A2-2*m3*cos(th3)*sin(th2)*rc3*dl-2*m2*sm(th2)*rc2*d0 

+4*m3*rc3A2*cos(th2)A2*cos(th3)A2-2*m3*sin(th2)*a2*d0 

-4*cos(th2)*sin(th3)*m3*rc3A2*cos(th3)*sin(th2) 

-4*cos(th2)*cos(th3)*I3yy*sin(th3)*sin(th2)+4*cos(th2)*cos(th3)*I3xx*sin(th3)*sin(th2) 

-4  *a2  *rc3  *m3  *cos(th3 )) 

NCMt0(2,7)=  -l/4*m3*rc3*a2*cos(th3+2*thl)+l/4*m3*rc3*a2*cos(-th3+2*thl) 

+1/4*13  yy*cos(-2*th2+2*thl~2*th3)4/4*I3xx*cos(-2*th2+2*thl-2*th3) 
4/4*m3*rc3*a2*cos(th3+2*th2+2*thl)+l/4*I3xx*cos(2*th2+2*thl+2*th3) 
4/4*I3yy*cos(2*th2+2*thl+2*th3)+l/4*m3*rc3A2*cos(-2*th2+2*thl-2*th3) 
4/4*ni3*rc3A2*cos(2*th2+2*thl+2*th3)+l/4*m3*rc3*a2*cos(-th3-2*th2+2*thl) 

NCMt0(2,8)=  2*m3*rc3*d0*cos(th3+th2)4/2*I3xx*sin(2*th3+2*th2) 
+l/4*m3*rc3A2*sin(-2*th2+2*thl-2*th3)4/4*m3*rc3A2*sin(2*th2+2*thl+2*th3) 
-3/2*m3*sin(th3)*rc3*a2+l/2*m3*rc3A2*sin(2*th3+2*th2)  +l/2*I3yy*sin(2*th3+2*th2) 
+2*m3*rc3*dl*cos(th3+th2)4/4*ni3*rc3*a2*sin(th3+2*th2+2*thl) 
+l/4*m3*rc3*a2*sin(-th3-2*th2+2*thl)-l/4*I3yy*sin(2*th2+2*thl+2*th3) 

+l/4*I3yy*sin(-2*th2+2*thl  -2*th3)+ 1  /2*m3  *rc3  *a2*sin(th3+2*th2) 

4/4*m3*rc3*a2*sin(th3+2*thl)+l/4*m3*rc3*a2*sin(-th3+2*thl)+l/4*I3xx*sm(2*th2+2*thl+2*th3) 

-  l/4*I3xx*sin(-2*th2+2*thl  -2*th3) 

NCMt0(2,9)=  m3*rc3*d0*cos(-th2+thl-th3)-m3*rc3*d0*cos(th2+thl+th3) 

-m3*rc3*dl*cos(th2+thl+th3)+m3*rc3*dl*cos(-th2+thl-  th3)+sin(thl)*I3zz+sin(thl)*m3*rc3A2 

+l/2*m3*rc3*a2*sin(thl+tB)+l/2*m3*rc3*a2*sin(thl-th3)-l/2*m3*rc3A2*sin(2*th2+thl+2*th3) 

-l/2*m3*rc3A2*sin(-2*th2+thl-2*th3)+l/2*I3xx*sin(2*th2+thl+2*th3)+l/2*I3xx*sin(-2*th2+thl-2*th3) 

4/2*I3yy*sin(2*th2+thl+2*th3)4/2*I3yy*sin(-2*th2+thl-2*th3)4/2*m3*rc3*a2*sin(2*th2+thl+th3) 

-l/2*m3*rc3*a2*sin(-2*th2+thl-th3) 

NCMt0(3,l)=0 


NCMto(3,2)=0 


NCMt0(3,3)=0 
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NCMx0(3,4)=  cos(thl)*(2*m3*rc3A2*cos(th3)A2+2*m3*rc3A2*cos(th2)A2-2*m2*cos(th2)A2*rc2A2 

-4*I3yy*cos(th3)A2*cos(th2)A2+4*I3xx*cos(th3)A2*cos(th2)A2-2*m3*cos(th2)A2*a2A2-2*m3*rc3A2 

-2*I2yy*cos(th2)A2+2*I2xx*cos(th2)A2-2*I3xx*cos(th2)A2+2*I3yy*cos(th2)A2+2*I3yy*cos(th3)A2 

-2*I3xx*cos(th3)A2-4*sin(th2)*sin(th3)*I3xx*cos(th3)*cos(th2) 

+4*sin(th2)*sin(th3)*I3yy*cos(th3)*cos(th2)-  4*cos(th3)*m3*rc3*cos(th2)A2*a2 

+4*sm(th2)*sin(th3)*m3*rc3*cos(th2)*a2+4*sin(th2)*sin(th3)*m3*rc3A2*cos(th3)*cos(th2)+I3xx-I3yy 

-4*m3*rc3A2*cos(th3)A2*cos(th2)A2-I3zz-I2zz  H2yy-I2xx) 

NCMt0(3,5)=  -sin(thl)*(2*m3*rc3A2-I3xx+I3yy+I3zz-2*in3*rc3A2*cos(th3)A2 

+4*I3yy*cos(th3)A2*cos(th2)A2+2*m3*cos(th2)A2*a2A2-4*I3xx*cos(th3)A2*cos(th2)A2 

-4*sin(th2)*sin(th3)*m3*rc3A2*cos(th3)*cos(th2)+4*sin(th2)*cos(th3)*I3xx*sin(th3)*cos(th2) 

+2*I3xx*cos(th2)A2-4*sin(th2)*cos(th3)*I3yy*sin(th3)*cos(th2)-  4*sin(th2)*sm(th3)*m3*rc3*cos(th2)*a2 

+4*cos(th3)*m3*rc3*cos(th2)A2*a2-2*I2xx*cos(th2)A2-2*I3yy*cos(th3)A2-2*I3yy*cos(th2)A2 

+2*I3xx*cos(th3)A2+2*I2yy*cos(th2)A2+4*m3*rc3A2*cos(th3)A2*cos(th2)A2+2*m2*cos(th2)A2*rc2A2 

-2*m3*rc3A2*cos(th2)A2+I2zz-I2yy+I2xx) 

NCMto(3,6)=  I3xx*sm(2*th3+2*th2)-m3*rc3A2*sin(2*th3+2*th2)-I3yy*sm(2*th3+2*th2) 

-2*m3*rc3*a2*sin(th3+2*th2)+I2xx*sin(2*th2)-m3*a2A2*sm(2*th2)-I2yy*sin(2*th2) 

-m2*rc2A2*sin(2*th2) 


NCMt0(3,7)=  -I3zz*cos(thl)-l/2*m3*rc3*a2*cos(2*th2+thl+th3)-m3*rc3A2*cos(thl) 
-l/2*m3*rc3A2*cos(-2*th2+thl-2*th3)-l/2*m3*rc3A2*cos(2*th2+thl+2*th3) 
-l/2*m3*rc3*a2*cos(-2*th2+thl-th3)-l/2*m3*rc3*a2*cos(thl-th3)-  l/2*m3*rc3*a2*cos(thl+th3) 
+l/2*I3xx*cos(-2*th2+thl-2*th3)+l/2*I3xx*cos(2*th2+thl+2*th3)-l/2*I3yy*cos(-2*th2+thl-2*th3) 
-1/2*13  yy*cos(2*th2+thl+2*th3) 

NCMx0(3,8)=  l/2*I3xx*sin(-2*th2+thl-2*th3)-l/2*m3*rc3*a2*sin(2*th2+thl+th3) 

-l/2*m3*rc3*a2*sin(-2*th2+thl-th3)-l/2*m3*rc3*a2*sin(thl+th3)-l/2*m3*rc3*a2*sin(thl-th3) 

-I3zz*sin(thl)+l/2*I3xx*sin(2*th2+thl+2*th3)-l/2*I3yy*sin(2*th2+thl+2*th3) 

-  l/2*I3yy*sin(-2*th2+thl -2*th3)- l/2*m3*rc3  A2*sin(-2*th2+thl -2*th3) 
-l/2*m3*rc3A2*sin(2*th2+thl+2*th3)-m3*rc3A2*sin(thl) 

NCMt0(3,9)=-m3*sm(th3)*rc3*a2-m3*rc3*a2*sin(th3+2*th2)+I3xx*sin(2*th3+2*th2) 
-m3*rc3A2*sin(2*th3+2*th2)-I3yy*sin(2*th3+2*th2)  (A-22) 


A.  1.3.  Joint  Torque  Effects 


Rigid  Inertia  Joint  Torques 


E+L— (G+E+J— K)(?2~  (E+J)c%  +1c2c.2i\N +(E + J)c^  0  0 


B. 


0 

0 


E+H+K+INc,  E+I3a+Nc} 
E+ha+Nc2  E+Iia 


(A-23) 
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Rigid  Coriolis  Joint  Torques 


NRt(l,l)=  -Ksin(2*th2)-I2yy*sin(2*th2)-E*sin(2*th3+2*th2)-  2*B*a2*sin(2*th2+th3) 
+I2xx*sin(2*th2)-I3yy*sin(2*th3+2*th2)+I3xx*sin(2*th3+2*th2) 

NR,(1,2)=  -B*sin(th3)*a2-E*sin(2*th3+2*th2)-B*a2*sin(2*th2+th3)  -J*sin(2*th3+2*th2) 

NRl(2,3)=  -2*sin(th3)*B*a2 

NRt(l,3)=NR,(2,l)=NRT(2,2)=NRT(3,l)=NR,(3,2)=NRT(3,3)=0  (A-24) 


Rigid  Centrifugal  Joint  Torques 

NR,(2,1)=  l/2*(E+J)*sin(2*th3+2*th2)+l/2*(K-G)*sin(2*th2)  +B*a2*sin(2*th2+th3) 

NRI(2,3)=  -sin(th3)*B*a2 

NRt(3,l)=  l/2*(E+J)*sin(2*th3+2*th2)  +l/2*sin(th3)*B*a2+l/2*B*a2*sin(2*th2+th3) 

NRt(3,2)=  sin(th3)*B*a2 

NCt(  1 , 1  )=NRt(  1 ,2)=NRX(  1 ,3)=NRX(2,2)=NRI(3,3)=0  (A-25) 


Rigid  Gravitational  Joint  Torques 


0 


Bg  cos(02  +  02 )  +  Ag  cos  02 


Bg  cos(<92  +  <93 ) 


Flexible  Body  Inertia  Joint  Torques 

At(l,l)=  -sin(thl)*(-B*sin(th3)*sin(th2)+cos(th2)*B*cos(th3)+A*cos(th2)) 
At(1,2)=  cos(thl)*(-B*sm(th3)*sin(th2)+cos(th2)*B*cos(th3)+A*cos(th2)) 
At(l,3)=0 

Ax(2,l)=  -cos(thl)*(B*sin(th3)*cos(th2)+B*cos(th3)*sin(th2)+A*sin(th2)) 
A,(2,2)=  -sin(thl)*(B*sin(th3)*cos(th2)+B*cos(th3)*sm(th2)+A*sin(th2)) 
AT(2,3)=  B*cos(th2+th3)+A*cos(th2) 

A,(3,l)=  -B*cos(thl)*(sin(th3)*cos(th2)+cos(th3)*sin(th2)) 

At(3,2)=  -B*sin(thl)*(sin(th3)*cos(th2)+cos(th3)*sin(th2)) 

Ax(3,3)=  B*cos(th2+th3) 


(A-26) 


(A-27) 


Flexible  Body  Coriolis  Joint  Torques 

NRCt(l,l)=  (2*cos(thl)A2-l)*(2*I3yy*cos(th2)A2*cos(th3)A2-2*sin(th2)*cos(th3)*I3yy*sin(th3)*cos(th2) 
+m3*cos(th2)A2*a2A2-2*I3xx*cos(th3)A2*cos(th2)A2+I3xx*cos(th3)A2-nB*rc3A2*cos(th2)A2 
-m3*rc3A2*cos(th3)A2+Ilzz-I3xx-I2yy+I2yy*cos(th2)A2-I2xx*cos(th2)A2+I2zz+I3zz-  I3yy*cos(th2)A2 
+m3*rc3A2+I3xx*cos(th2)A2-I3yy*cos(th3)A2-  Ilxx+2*m3*rc3A2*cos(th3)A2*cos(th2)A2 
+m2*cos(th2)A2*rc2A2+2*cos(th3)*m3*rc3*cos(th2)A2*a2+2*sin(th2)*sin(th3)*I3xx*cos(th3)*cos(th2) 
-2*sin(th2)*sin(th3)*in3*rc3*cos(th2)*a2-2*sin(th2)*sin(th3)*m3*rc3A2*cos(th3)*cos(th2)) 

NRCT(l,2)=-sin(thl)*(-  cos(th3)*I3yy*sin(th3)+sin(th2)*I2yy*cos(th2)+sin(th2)*I3xx*cos(th2) 
+sin(th3)*I3xx*cos(th3)-  cos(th2)*I3yy*sin(th2)+2*cos(th3)*m3*rc3A2*sin(th3)*cos(th2)A2 
+cos(th2)*rc2*m2*d0+cos(th2)*m3*dl*a2-2*cos(th3)*I3xx*sin(th3)*cos(th2)A2 
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+cos(th2)*cos(th3)*m3*rc3*d0+2*m3*sin(th3)*cos(th2)A2*rc3*a2+cos(th2)*cos(th3)*m3*rc3*dl 

+2*cos(th2)*cos(th3)*m3*rc3*sin(th2)*a2+cos(th2)*m3*sin(th2)*a2A2+cos(th2)*m2*sm(th2)*rc2A2 

-sin(th2)*m3*rc3A2*cos(th2)-2*cos(th2)*I3xx*cos(th3)A2*sin(th2)-sin(th2)*sin(th3)*m3*rc3*d0 

-Sin(th2)*sin(th3)*m3*rc3*dl-sin(th3)*m3*rc3*a2-  sin(th3)*m3*rc3A2*cos(th3)+cos(th2)*m3*dO*a2 

+2*sin(th3)*I3yy*cos(th3)*cos(th2)A2+2*sin(th2)*I3yy*cos(th3)A2*cos(th2) 

+2*cos(th2)*m3*rc3A2*cos(th3)A2*sin(th2)-cos(th2)*I2xx*sin(th2)+cos(th2)*rc2*m2*dl) 

NRCt(1,3)=  cos(thl)*(-cos(th2)*I3yy*sin(th2)+sin(th2)*I3xx*cos(th2)-cos(th3)*I3yy*sin(th3) 

-  cos(th2)*I2xx*sin(th2)+sin(th2)*I2yy*cos(th2)+sin(th3)*I3xx*cos(th3) 
+2*cos(th3)*m3*rc3A2*sin(th3)*cos(th2)A2-sin(th3)*m3*rc3A2*cos(th3)-  sin(th3)*m3*rc3*a2 
+2*m3*sin(th3)*cos(th2)A2*rc3*a2-sin(th2)*sin(th3)*m3*rc3*dl-  sin(th2)*m3*rc3A2*cos(th2) 
+2*sin(th3)*I3yy*cos(th3)*cos(th2)A2-2*cos(th3)*I3xx*sin(th3)*cos(th2)A2 
-2*cos(th2)*I3xx*cos(th3)A2*sin(th2)-  sin(th2)*sin(th3)*m3*rc3*d0 

+2*cos(th2)*cos(th3)*m3*rc3*sin(th2)*a2+cos(th2)*m2*sin(th2)*rc2A2+cos(th2)*rc2*m2*dl 

+Cos(th2)*rc2*m2*d0+cos(th2)*m3*d0*a2+cos(th2)*m3*dl*a2+cos(th2)*cos(th3)*m3*rc3*dl 

+cos(th2)*cos(th3)*m3*rc3*d0+2*cos(th2)*m3*rc3A2*cos(th3)A2*sin(th2)+cos(th2)*m3*sin(th2)*a2A2 

+2*sin(th2)*I3yy*cos(th3)A2*cos(th2)) 

NRCX(2,1)=  4/4*m3*rc3A2*cos(2*th2-2*thl+2*th3)+l/4*m3*rc3A2*cos(2*th2+2*thl+2*th3) 
4/4*I3xx*cos(2*th2+2*thl+2*th3)+l/4*I3yy*cos(2*th2+2*thl+2*th3) 

4/4*I3yy*cos(2*th2-2*thl+2*th3)+l/4*I3xx*cos(2*th2-2*thl+2*th3)+l/4*I2yy*cos(2*thl+2*th2) 

4/4*I2xx*cos(2*thl+2*th2)+l/4*I2xx*cos(-2*thl+2*th2)4/4*I2yy*cos(-2*thl+2*th2) 

+l/4*m3*a2A2*cos(2*thl+2*th2)4/4*m3*a2A2*cos(~2*thl+2*th2)+l/4*m2*rc2A2*cos(2*thl+2*th2) 

-l/4*m2*rc2A2*cos(-2*thl+2*th2)+l/2*m3*rc3*a2*cos(2*th2+th3+2*thl) 

-  l/2*m3*rc3*a2*cos(2*th2+th3-2*thl) 

NRCt(2,2)=  cos(thl)*(~  4*I3xx*cos(th3)A2*cos(th2)A2+4*I3yy*cos(th3)A2*cos(th2)A2 
+2*m2*cos(th2)A2*rc2A2-2*m3*rc3A2*cos(th3)A2+2*m3*cos(th2)A2*a2A2+m3*rc3A2-m2*rc2A2 
-2*cos(th2)A2*I2xx+2*I3xx*cos(th2)/s2-2*I3yy*cos(th2)A2+2*I3xx*cos(th3)A2-I3xx+I2xx-I2yy+I3yy 
-4*m3*rc3A2*cos(th3)*cos(th2)*sin(th3)*sin(th2)-  4*m3*rc3*sin(th3)*cos(th2)*a2*sin(th2) 
+4*m3*rc3*cos(th3)*cos(th2)A2*a2-4*I3yy*cos(th3)*cos(th2)*sin(th3)*sin(th2) 

-  m3*rc3*cos(th3)*sin(th2)*d0-m3*rc3*sin(th3)*cos(th2)*d0+4*I3xx*cos(th3)*cos(th2)*sin(th3)*sin(th2) 
-m3*rc3*cos(th3)*sin(th2)*dl-rc2*m2*sin(th2)*dl-iti3*a2A2-ni3*rc3*sin(th3)*cos(tli2)*dl 
-2*m3*rc3A2*cos(th2)A2-rc2*m2*sin(th2)*d0-m3*sin(th2)*dl*a2-2*m3*rc3*cos(th3)*a2 
-m3*sin(th2)*d0*a2+4*m3*rc3A2*cos(th3)A2*cos(th2)A2-2*I3yy*cos(th3)A2+2*cos(th2)A2*I2yy) 

NRCt(2}3)=  -  sin(thl)*(m3*a2A2+rc2*m2*sin(th2)>Kdl+2*I3yy,itcos(th3)A2+2*cos(th2)A2*I2xx 

+m3*rc3*cos(th3)*sin(th2)*d0+4*m3*rc3*sin(th3)*cos(th2)*a2*sin(th2)-2*I3xx*cos(th2)A2 

-2*I3xx*cos(th3)A2+nl3*rc3*sin(th3)*cos(th2)*dl-4*m3*rc3*cos(th3)*cos(th2)A2*a2 

+m3*rc3*cos(th3)*sin(th2)*dl-2*cos(th2)A2*I2yy+2*I3yy*cos(th2)A2 

+4*I3yy*cos(th3)*cos(th2)*sin(th3)*sin(th2)+4*m3*rc3A2*cos(th3)*cos(th2)*sin(th3)*sin(th2) 

+2*m3*rc3A2*cos(th3)A2-2*m3*cos(th2)A2*a2A2-4*I3yy*cos(th3)A2*cos(th2)A2-2*m2*cos(th2)A2*rc2A2 

+4*I3xx*cos(th3)A2*cos(th2)A2+2*m3*rc3A2*cos(th2)A2+2*m3*rc3*cos(th3)*a2+m3*sin(th2)*d0*a2 

+m3*sin(th2)*dl*a2+rc2*m2*sin(th2)*dO+I3xx-I2xx+I2yy-I3yy 

-4*I3xx*cos(th3)*cos(th2)*sin(th3)*sin(th2)  +m3*rc3*sin(th3)*cos(th2)*d0 

-4*m3*rc3A2*cos(th3)A2*cos(th2)A2+m2*rc2A2-m3*rc3A2) 

NRCX(3,1)=  l/4*m3*rc3A2*cos(2*th2+2*thl+2*th3)-l/4*m3*rc3A2*cos(2*th2-2*thl+2*th3) 

-l/4*m3*rc3*a2*cos(th3-2*thl)+l/4*m3*rc3*a2*cos(th3+2*thl)+l/4*I3yy*cos(2*th2+2*thl+2*th3) 

4/4*I3yy*cos(2*th2-2*thl+2*th3)+l/4*I3xx*cos(2*th2-2*thl+2*th3) 

-  !/4*I3xx*cos(2*th2+2*thl+2*th3)  +l/4*m3*rc3*a2*cos(2*th2+th3+2*thl) 
4/4*m3*rc3*a2*cos(2*th2+th3-2*thl) 
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NRCt(3,2)=  l/2*I3yy*cos(-thl+2*th3+2*th2)+l/2*I3yy*cos(thl+2*th3+2*th2) 
-l/2*I3xx*cos(4hl+2*1h3+2*th2)4/2*I3xx*cos(thl+2*th3+2*th2) 
+l/2*m3*rc3A2*cos(-thl+2*th3+2*th2)  +l/2*m3*rc3A2*cos(thl+2*th3+2*th2) 
-l/2*m3*rc3*dl*sm(th2+thl+th3)-l/2*m3*rc3*dl*sm(th2-thl+th3)-l/2*m3*rc3*d0*sin(th2+thl+th3) 
-l/2*m3*rc3*d0*sin(th2-thl+th3)+l/2*m3*rc3*a2*cos(2*th2-  thl+th3) 
+l/2*m3*rc3*a2*cos(2*th2+thl+th3) 

NRC,(3,3)=  l/2*m3*rc3*a2*sin(2*th2+thl+th3)-l/2*m3*rc3*a2*sin(2*th2-thl+th3) 
-l/2*m3*rc3*d0*cos(th2-thl+th3)+l/2*m3*rc3*d0*cos(th2+thl+th3)-l/2*I3yy*sin(-thl+2*th3+2*th2) 
+l/2*I3yy*sin(thl+2*th3+2*th2)+l/2*I3xx*sm(-thl+2*th3+2*th2)-l/2*I3xx*sin(thl+2*th3+2*th2) 
-l/2*m3*rc3*dl*cos(th2-thl+th3)+l/2*m3*rc3*dl*cos(th2+thl+th3) 

-l/2*nB*rc3A2*sin(-thl+2*th3+2*th2)  +l/2*rrB*rc3A2*sin(thl+2*th3+2*th2)  (A-28) 


Flexible  Body  Coriolis  Joint  Torques 

NCCX(1,1)=  -sin(thl)*cos(thl  )*(2*m3  *rc3  *cos(th3)*cos(th2)A2*a2 
-2*I3yy*cos(th3)*cos(th2)*sin(th3)*sin(th2)  +2*I3xx*cos(th3)*cos(th2)*sin(th3)*sin(th2) 
+m2*cos(th2)A2*rc2A2+m3*cos(th2)A2*a2A2+2*I3)^y*cos(th3)A2,,ccos(th2)A2 
-2*I3xx*cos(th3)A2*cos(th2)A2-m3*rc3A2*cos(th3)A2-tn3*rc3A2*cos(th2)A2+I3xx*cos(th3)A2 
+I3xx*cos(th2)A2+I2yy*cos(th2)A2-I2xx*cos(th2)A2-I3yy*cos(th2)A2-I3yy*cos(th3)A2 
-2*m3*rc3A2*cos(th3)*cos(th2)*sin(th3)*sin(th2)-  2*m3*rc3*sin(th3)*cos(th2)*a2*sin(th2) 
+2*m3  *rc3  A2*cos(th3)A2*cos(th2)A2+m3  *rc3  A2-I3xx-I2yy+I3zz+I2zz-I  lxx+1 1  zz) 

NCCX(1,2)=  sin(thl)*cos(thl)*(2*m3*rc3*cos(th3)*cos(th2)A2*a2 

>2*I3yy*cos(th3)*cos(th2)*sin(th3)*sin(th2)  +2*I3xx*cos(th3)*cos(th2)*sin(th3)*sin(th2) 

+m2*cos(th2)A2*rc2A2+nB*cos(tl£)A2*a2A2+2*I3yy*cos(th3)A2*cos(th2)A2 

-2*I3xx*cos(th3)A2*cos(th2)A2-m3*rc3A2*cos(th3)A2-nB*rc3A2*cos(th2)A2+I3xx*cos(th3)A2 

+I3xx*cos(th2)A2+I2yy*cos(th2)A2-I2xx*cos(th2)A2-I3yy*cos(th2)A2-I3yy*cos(th3)A2 

-2*m3*rc3A2*cos(th3)*cos(th2)*sin(th3)*sin(th2)-  2*m3*rc3*sin(th3)*cos(th2)*a2*sin(th2) 

+2*irB*rc3A2*cos(th3)A2*cos(th2)A2+m3*rc3A2-I3xx-I2yy+I3zz+I2zz-Ilxx+Ilzz) 


NCCX(1,3)=  0 

NCCX(2,1)=  -1/8*13 yy*sin(2*tli2-2*thl+2*th3)-  l/8*I3yy*sin(2*th2+2*thl+2*th3) 

+1/4*13  xx*sin(2*th3+2*th2)+l/8*I3xx*sin(2*th2-2*thl+2*th3)+l/8*I3xx*sin(2*th2+2*thl+2*th3) 

-m3*rc3*d0*cos(th2+th3)-l/4*m3*rc3A2*sin(2*th3+2*th2)-l/8*m3*rc3A2*sin(2*th2+2*thl+2*th3) 

4/8*m3*rc3A2*sin(2*th2-2*thl+2*th3)-rc2*m2*cos(th2)*dl-rc2*m2*cos(th2)*d0 

-l/2*m3*rc3*a2*sin(2*th2+th3)  -l/4*m3*rc3*a2*sin(2*th2+th3+2*thl) 

-l/4*m3*rc3*a2*sin(2*th2+th3~2*thl)  -m3*rc3*dl*cos(th2+th3)-l/4*I3yy*sin(2*th3+2*th2) 

-m3*cos(th2)*d0*a2-m3*cos(th2)*dl*a2-l/4*m3*a2A2*sin(2*th2)-l/8*m3*a2A2*sin(2*thl+2*th2) 

~l/8*m3*a2A2*sin(-2*thl+2*th2)-l/4*I2yy*sin(2*th2)-l/8*I2yy*sin(2*thl+2*th2) 

-l/8*I2yy*sin(-2*thl+2*th2)+l/8*I2xx*sin(2*thl+2*th2)+l/8*I2xx*sin(-2*thl+2*th2) 

+l/4*I2xx*sin(2*th2)-l/8*m2*rc2A2*sin(2*thl+2*th2)-l/8*m2*rc2A2*sin(-2*thl+2*th2) 

-l/4*m2*rc2A2*sin(2*th2) 

NCCX(2,2)=  l/8*I3yy*sin(2*th2+2*thl+2*th3)-l/4*I3yy*sin(2*th3+2*th2)+l/4*I3xx*sin(2*th3+2*th2) 
-l/8*I3xx*sin(2*th2-2*thl+2*th3)-l/8*I3xx*sin(2*th2+2*thl+2*th3)+l/8*I3yy*sin(2*th2-2*thl+2*th3) 
-rc2  *m2*cos(th2)  *d  1  -rc2  *m2  *cos(th2)*d0-m3  *rc3  *d  1  *  cos(th2+th3)- 1  /2*m3  *rc3  *a2*sin(2  *th2+th3) 
-m3*cos(th2)*d0*a2-m3*cos(th2)*dl*a2-  m3*rc3*d0*cos(th2+th3) 
+l/4*m3*rc3*a2*sin(2*tli2+th3+2*thl)+l/4*m3*rc3*a2*sin(2*th2+th3-2*thl) 
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-l/8*I2xx*sin(2*thl+2*th2)-l/8*I2xx*sin(-2*thl+2*th2)+l/4*I2xx*sin(2*th2) 

+l/8*m3*a2A2*sin(2*thl+2*th2)  +l/8*m3*a2A2*sm(-2*thl+2*th2)-l/4*m3*a2A2*sin(2*th2) 

4/4*I2yy*sin(2*th2)+l/8*I2yy*sin(2*thl+2*th2)+l/8*I2yy*sin(>2*thl+2*th2) 

-l/4*m3*rc3A2*sin(2*th3+2*th2)+l/8*m2*rc2A2*sin(2*thl+2*th2)+l/8*m2*rc2A2*sin(-2*thl+2*th2) 

-l/4*m2*rc2A2*sin(2*th2)+l/8*riB*rc3A2*sin(2*th2+2*thl+2*th3) 

+l/8*m3*rc3A2*sin(2*th2-2*thl+2*th3) 

NCCX(2,3)=  l/2*m2*rc2A2*sin(2*th2)4-l/2*m3*a2A2*sin(2*th2)+l/2*m3*rc3A2*sin(2*th3+2*th2) 
-l/2*I3xx*sin(2*th3+2*th2)-  l/2n2xx*sin(2*th2)+m3*rc3*a2*sin(2*th2+th3) 
+l/2*I3yy*sin(2*th3+2*th2)+l/2*I2yy*sin(2*th2) 

NCCX(3,1)“  -l/4*m3*rc3A2*sin(2*th3+2*th2)-l/8*m3*rc3A2*sin(2*th2-2*thl+2*th3) 
4/8*m3*rc3A2*sin(2*th2+2*thl+2*th3)+l/4*I3xx*sin(2*th3+2*th2)+l/8*I3xx*sin(2*th2+2*thl+2*th3) 
+l/8*I3xx*sin(2*th2-2*thl+2*th3)-l/8*m3*rc3*a2*sin(th3-2*thl)-l/8*I3yy*sin(2*th2-2*thl+2*th3) 
-m3*rc3*dl*cos(th2+th3)-l/8*m3*rc3*a2*sin(th3+2*thl)+3/4*m3*rc3*a2*sin(th3) 

-  m3*rc3*d0*cos(th2+th3)  -l/8*m3*rc3*a2*sin(2*th2+th3-2*thl)-l/8*m3*rc3*a2*sin(2*th2+th3+2*thl) 
-l/8*I3yy*sin(2*th2+2*thl+2*th3)-l/4*I3yy*sin(2*th3+2*th2)-l/4*m3*rc3*a2*sin(2*th2+th3) 

NCCX(3,2)=  3/4*m3*rc3*a2*sin(th3)-  l/4*m3*rc3A2*sin(2*th3+2*th2) 
+l/8*m3*rc3A2*sin(2*th2+2*thl+2*th3)  +l/8*nB*rc3A2*sin(2*th2-2*thl+2*th3) 
-l/4*I3yy*sin(2*th3+2*th2)+l/8*I3yy*sin(2*th2+2*thl+2*th3)+l/8*I3yy*sin(2*th2-2*thl+2*th3) 
-l/4*m3*rc3*a2*sin(2*th2+th3)+l/8*m3*rc3*a2*sin(2*th2+th3+2*thl) 

+l/8*m3*rc3*a2*sin(2*th2+th3-2*thl)+l/8*m3*rc3*a2*sin(th3+2*thl)+l/8*m3*rc3*a2*sin(th3-2*thl) 

-m3*rc3Ml*cos(th2+th3)-l/8*I3xx*sin(2*th2+2*thl+2*th3)4/8*I3xx*sin(2*th2-2*thl+2*th3) 

-m3*rc3*d0*cos(th2+th3)+l/4*I3xx*sin(2*th3+2*th2) 

NCCX(3,3)=  l/2*m3*rc3*a2*sin(th3)+l/2*m3*rc3*a2*sin(2*th2+th3)+l/2*m3*rc3A2*sin(2*th3+2*th2) 
-l/2*I3xx*sin(2*th3+2*th2)+l/2*I3yy*sin(2*th3+2*th2)  (A-29) 

Flexible  Body  Rotational  Inertia  Torques 


BWx(l,l)=  -cos(thl)*(-sin(th3)*I3yy*cos(th3)-  a2*rc3*sin(th3)*m3+2*m3*sin(th3)*cos(th2)A2*rc3*a2 
+cos(th2)*cos(th3)*m3*rc3*dl+cos(th2)*cos(th3)*m3*rc3*d0-  sin(th3)*rc3A2*m3*cos(th3) 
+cos(th2)*rc2*m2*d0+2*cos(th3)*m3*rc3A2*sin(th3)*cos(th2)A2 

+2*cos(th2)*cos(th3)*m3*rc3*sin(th2)*a2+cos(th2)*rc2*m2*dl-  sin(th2)*m3*rc3A2*cos(th2) 
+2*cos(th3)*I3yy*sin(th3)*cos(th2)A2-  sin(th2)*sin(th3)*m3*rc3*d0 

+2*cos(th2)*m3*rc3A2*cos(th3)A2*sin(th2)-  sin(th2)*sin(th3)*m3*rc3*dl+cos(th2)*m3*dl,,ca2 
+2*cos(th2)*I3yy*cos(th3)A2*sin(th2)-2*I3xx*sin(th3)*cos(th3)*cos(th2)A2+cos(th2)*m2*sin(th2)*rc2A2 
-2*sin(th2)*I3xx*cos(th3)A2*cos(th2)+sin(th2)5,cI3xx*cos(th2)-  cos(th2)*I3yy*sin(th2) 
+cos(th2)*I2yy*sin(th2)  -sin(th2)*I2xx*cos(th2)+cos(th3)*sin(th3)*I3xx 
+cos(th2)*m3*sin(th2)*a2A2+cos(th2)*m3*d0*a2) 


BWx(l,2)=  -sin(thl)*(-sin(th3)*rc3A2*m3*cos(th3)-  a2*rc3*sin(th3)*m3 

4-2*cos(th2)*cos(th3)*m3*rc3*sin(th2)*a2~sin(th2)5,tsin(th3)*ni3*rc3*d0 

+2*cos(th2)*m3*rc3A2*cos(th3)A2*sin(th2)“  sin(th2)*sin(th3)*m3*rc3*dl 

+2*m3*sin(th3)*cos(th2)A2*rc3*a2+2*cos(th3)*m3*rc3A2*sin(th3)*cos(th2)A2+cos(th2)*rc2*m2*dl 

-2*I3xx*sin(th3)*cos(th3)*cos(th2)A2-sin(th2)*m3*rc3A2*cos(th2)-  2*sin(th2)*I3xx*cos(th3)A2*cos(th2) 

+2*cos(th3)*I3yy*sin(th3)*cos(th2)A2+2*cos(th2)*I3yy*cos(th3)A2*sm(th2)+cos(th2)*m3*dl*a2 

+cos(th2)*cos(th3)*m3*rc3*dl+cos(th2)*cos(th3)*m3*rc3*d0+cos(th2)*rc2*m2*d0 

+cos(th2)*m2*sin(th2)*rc2A2+cos(th2)*m3*d0*a2+cos(th2)*m3*sin(th2)*a2A2+cos(th2)*I3xx*sin(th2) 

+cos(th2)*I2yy*sin(th2)-sin(th2)*I2xx*cos(th2)+cos(th3)*siii(th3)*I3xx-sin(th3)*I3yy*cos(th3) 
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-cos(th2)*I3yy*sin(th2)) 


BWt(1,3)=  l/2*I2xx+l/2*I2yy4/2*I3xx*cos(2*th3+2*th2)+m3*rc3*a2*cos(2*th2+th3) 
+l/2*m3*rc3A2*cos(2*th3+2*th2)+l/2*I3yy*cos(2*th3+2*th2)+Ilyy+l/2*m3*rc3A2+l/2*I3xx 
+l/2*I2yy*cos(2*th2)+l/2*I3yy-l/2*I2xx*cos(2*th2)+l/2*m3*a2A2+l/2*m3*a2A2*cos(2*th2) 
+cos(th3)*m3*rc3*a2+l/2*m2*rc2A2+l/2*m2*rc2A2*cos(2*th2) 

BWt(2,1)=  sin(thl)*(I3zz+m3*sin(th2)*dl*a2+m3*sin(th2)*d0*a2+rc2*m2*sin(th2)*d0 
+rc2*m2*sin(th2)*dl+m2*rc2A2+m3*a2A2+in3*rc3*sin(th3)*cos(th2)*dl+ni3*rc3*cos(th3)*sin(th2)*d0 
+rn3  *rc3*cos(th3)*sin(th2)*dl+2*cos(th3)*m3  *rc3  *a2+m3  *rc3  *sin(th3)*cos(th2)*d0+m3*rc3  A2+I2zz) 

BWt(252)=  -  Cos(thl)*(13zz+m3*sin(th2)*dl*a2+m3*sin(th2)*d0*a2+rc2*m2*sin(th2)*d0 

+rc2*m2*sin(th2)*dl+m2*rc2A2+m3*a2A2+m3*rc3*sin(th3)*cos(th2)*dl+m3*rc3*cos(th3)*sin(th2)*d0 

+m3*rc3*cos(th3)*sin(th2)*dl+2*cos(th3)*m3*rc3*a2+m3*rc3*sin(th3)*cos(th2)*d0+m3*rc3A2+I2zz) 

BWt(3,1):=  sin(thl)*(m3*rc3*sin(th3)*cos(th2)*d0+m3*rc3*sin(th3)*cos(th2)*dl 
+m3*rc3*cos(th3)*sm(th2)*d0+m3*rc3*cos(th3)*sin(th2)*dl+m35,crc3*cos(th3)*a2+m3*rc3A2+I3zz) 

BWx(3,2)=  -  cos(thl)*(d0*cos(th2)*sin(th3)*rc3*m3+dl*cos(th2)*sin(th3)*rc3*m3 
+d0*sin(th2)*cos(th3)*rc3*m3+dl*sin(th2)*cos(th3)*rc3*in3+a2*cos(th3)*rc3*ni3+rc3A2*m3+I3zz) 

Bwx(2,3)=BWt(3,3)=0  (A-30) 


Cross  Coupling  Interaction  Torque  Effects 


NcMx(l,l)=NcMx(l>2)=NcM,(l,3)=NcMt(2,4)=NCMx(2,5)=NcMx(2)6)=NcMx(2)9)=NcMx(3,6)=NcM,(3,7)= 

Ncmx(3,8)=NCmx(3,9)=0 


NCmx(1>4)=  cos(thl)*(-I2xx-I3zz-2*rc3A2*m3+2*cos(th3)A2*rc3A2*m3-2*m2*cos(th2)A2*rc2A2 

+2*m3*rc3A2*cos(th2)A2-2*m3*cos(th2)A2*a2A2+4*I3xx*cos(th3)A2*cos(th2)A2 

-4*I3yy*cos(th3)A2*cos(th2)A2-I3yy-I2zz+I2yy+I3xx+2*cos(th3)A2*I3yy-2*cos(th3)A2*I3xx 

+2*I2xx*cos(th2)A2-2*I2yy*cos(th2)A2+2*I3yy*cos(th2)A2-2*I3xx*cos(th2)A2 

+4*sin(th2)*sin(th3)*I3yy*cos(th3)*cos(th2)-4*sin(th2)*sm(th3)*I3xx*cos(th3)*cos(th2) 

-4*cos(th3)*m3*rc3*cos(th2)A2*a2-4*m3*rc3A2*cos(th3)A2*cos(th2)A2 

+4*sin(th2)*sin(th3)*m3*rc3*cos(th2)*a2+4*sin(th2)*sin(th3)*m3*rc3A2*cos(th3)*cos(th2)) 

Ncmx(1>5)=  sin(thl)*(-I2xx-  I3zz+4*sm(th2)*sm(th3)*m3*rc3A2*cos(th3)*cos(th2) 

+4*sin(th2)*cos(th3)*I3yy*sin(th3)*cos(th2)-  4*sin(th2)*cos(th3)*I3xx*sin(th3)*cos(th2) 

+2*m3*rc3A2*cos(th2)A2+2*m3*rc3A2*cos(th3)A2-4*I3yy*cos(th3)A2*cos(th2)A2 

+4*I3xx*cos(th3)A2*cos(th2)A2-2*m2*cos(th2)A2*rc2A2-2*m3*cos(th2)A2*a2A2-2*m3*rc3A2 

-4*cos(th3)*m3*rc3*cos(th2)A2*a2-2*I3xx*cos(th3)A2+4*sm(th2)*sm(th3)*m3*rc3*cos(th2)*a2 

-2*I2yy*cos(th2)A2-I3yy-I2zz+I2yy+I3xx-2*I3xx*cos(th2)A2+2*I3yy*cos(th2)A2+2*I3yy*cos(th3)A2 

+2*I2xx*cos(th2)A2-4*m3*rc3A2*cos(th3)A2*cos(th2)A2) 


NCMx(t>6)=  -m3*a2A2*sin(2*th2)-m2*rc2A2*sin(2*th2)-I3yy*sin(2*th3+2*th2) 

-m3*rc3A2*sin(2,|‘th3+2*th2)+I3xx*sin(2*th3+2*th2)-I2yy*sin(2*th2)+I2xx*sm(2*th2) 

-2*m3*rc3*a2*sin(2*th2+th3) 

NCmx(1,7)=  -l/2*m3*rc3A2*cos(thl+2*th3+2*th2)-l/2*m3*rc3A2*cos(-thl+2*th3+2*th2) 
+l/2*I3xx*cos(-thl+2*th3+2*th2)+l/2*I3xx*cos(thl+2*th3+2*th2)-l/2*I3yy*cos(thl+2*th3+2*th2) 
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-l/2*I3yy*cos(“thl+2*th3+2*th2)-m3*rc3A2*cos(thl)-I3zz*cos(thl)-l/2*m3*rc3*a2*cos(2*th2-thl+th3) 

-l/2*m3*rc3*a2*cos(2*th2+thl+th3)-l/2*m3*rc3*a2*cos(-thl+th3)-l/2*m3*rc3*a2*cos(thl+th3) 

Ncmt(1,8)=  l/2*m3*rc3*a2*sin(2*th2-thl+th3)-l/2*m3*rc3*a2*sin(2*th2+thl+th3) 
-l/2*m3*rc3*a2*sin(thl+th3)+l/2*m3*rc3*a2*sin(-thl+th3)-m3*rc3A2*sin(thl)-I3zz*sin(thl) 
4/2*I3yy*sin(thl+2*th3+2*th2)+l/2*I3yy*sin(-thl+2*th3+2*th2) 
+l/2*m3*rc3A2*sin(-thl+2*th3+2*th2)-  l/2*m3*rc3A2*sin(thl+2*th3+2*th2) 
+l/2*I3xx*sin(thl+2*th3+2*th2)-l/2*I3xx*sin(-thl+2*th3+2*th2) 


Ncmt(159)=  -m3*rc3A2*sin(2*th3+2*th2)-m3*rc3*a2*sm(2*th2+th3)-m3*sin(th3)*rc3*a2 
-I3yy*sin(2*th3+2*th2)+I3xx*sin(2*th3+2*th2) 


Ncmx(2,1)=  cos(thl)*(2*m3*rc3A2+I3yy-I3xx-I2yy+I2xx+I2zz 

-4*m3*rc3A2*cos(th3)*sin(th2)*sin(th3)*cos(th2)+4s,cm3*rc3*cos(th3)*cos(th2)A2*a2 

-4*I3yy*cos(th3)*sin(th2)*sin(th3)*cos(th2)+4*I3xx*cos(th3)*sin(th2)*sin(th3)*cos(th2) 

-4*m3*rc3*sin(th3)*sin(th2)*a2*cos(th2)-2*m3*rc3A2*cos(th2)A2-2*m3*rc3A2*cos(th3)A2 

+4*I3yy*cos(th3)A2*cos(th2)A2-4*I3xx*cos(th3)A2*cos(th2)A2-2*cos(th2)A2*I2xx-2*I3yy*cos(th2)A2 

-2*I3yy*cos(th3)A2+2*cos(th2)A2*I2yy+2*I3xx*cos(th2)A2+2*I3xx*cos(th3)A2+2*m2*cos(th2)A2*rc2A2 

+2*m3*cos(th2)A2*a2A2+4*m3*rc3A2*cos(th3)A2*cos(th2)A2+I3zz) 

NCmt(2,2)=  sin(thl)*(2*I3xx*cos(th2)A2-2*cos(th2)A2*I2xx+2*cos(th2)A2*I2yy'2*I3yy*cos(th3)A2 

-2*I3yy*cos(th2)A2+2*m3*rc3A2+I3yy“I3xx-I2yy+I2xx-2*m3*rc3A2*cos(th2)A2+2*m3*a2A2*cos(th2)A2 

+2*m2*cos(th2)A2*rc2A2-4*I3yy*cos(th3)*sin(th2)*sin(th3)*cos(th2)+4*I3yy*cos(th3)A2*cos(th2)A2 

+4*I3xx*cos(th3)*sin(th2)*sin(th3)*cos(th2)+4*m3*rc3*cos(th3)*cos(th2)A2*a2+2*I3xx*cos(th3)A2 

-4*m3*rc3A2*cos(th3)*sin(th2)*sin(th3)*cos(th2)-4*m3*rc3*sin(th3)*sin(th2)*a2*cos(th2) 

-4*I3xx*cos(th3)A2*cos(th2)A2-2*m3*rc3A2*cos(th3)A2+4*m3*rc3A2*cos(th3)A2*cos(th2)A2+I2zz+I3zz) 

NCmx(2,3)=  I3yy*sin(2*th3+2*th2)-I2xx*sin(2*th2)-  I3xx*sin(2*th3+2*th2)+m2*rc2A2*sin(2*th2) 
+I2yy*sin(2*th2)+m3*rc3A2*sin(2*th3+2*th2)+2*m3*rc3*a2*sin(2*th2+th3)+m3*a2A2*sin(2*th2) 


NCmx(2,7)=  -2*m3*sin(th3)*sm(thl)*a2*rc3 


NCmx(2,8)=  2*m3*sin(th3)*cos(thl)*a2*rc3 

Ncmx(3,1)=  m3*rc3A2*cos(thl)+l/2*m3*a2*rc3*cos(-thl+th3)+l/2*m3*a2*rc3*cos(thl+th3) 
+I3zz*cos(thl)+l/2*m3*a2*rc3*cos(2*th2-  thl+th3)+l/2*m3*a2*rc3*cos(2*th2+thl+th3) 
+l/2*m3*rc3A2*cos(thl+2*th3+2*th2)+l/2*m3*rc3A2*cos(-thl+2*th3+2*th2) 
-l/2*I3xx*cos(-thl+2*th3+2*th2)4/2*I3xx*cos(thl+2*th3+2*th2)+l/2*I3yy*cos(4lil+2*th3+2*th2) 
+l/2*I3yy*cos(thl+2*th3+2*th2) 

NCmx(3,2)=  1/2*13 xx*sin(-thl+2*th3+2*th2)-  l/2*I3xx*sin(thl+2*th3+2*th2)+m3*rc3A2*sin(thl) 

+I3zz*sin(thl)+l/2*m3*rc3*a2*sin(2*th2+thl+th3)-l/2*m3*rc3*a2*sin(2*th2-thl+th3) 

-l/2*m3*rc3*a2*sin(-thl+th3)+l/2*m3*rc3*a2*sin(thH-th3)+l/2*I3yy*sin(thl+2*th3+2*th2) 

-l/2*I3yy*sin(-thl+2*th3+2*th2)-l/2*m3*rc3A2*sin(-thl+2*th3+2*tli2) 

+l/2*m3*rc3A2*sin(thl+2*th3+2*th2) 

Ncmx(3,3)=  m3*rc3*sin(th3)*a2+m3*rc3*a2*sin(2*th2+th3)+m3*rc3A2*sin(2*th3+2*th2) 
+I3yy*sin(2*th3+2*th2)-I3xx*sin(2*th3+2*th2) 


Ncmx(3  ,4)=  2  *m3  *  sin(th3 )  *sin(th  1 )  *  a2  *rc3 

NCmx(3,5)=  -2*m3*sin(th3)*cos(thl)*a2*rc3  (A-3 1) 
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A.2  Three  Degree  of  Freedom  Spherical  Robot 


The  spherical  robot  was  chosen  as  an  alternate  configuration  for  three  reasons.  First,  it 
is  a  common  robot  configuration  and  modeling  information  is  easily  found  in  many 
robotics  texts,  including  Craig  [16]  and  Sciavicco  and  Siciliano  [59] 


Table  A-2 

Denavit-Hartenberg  Parameters  for  Spherical  Robot 


Link 

Oti 

di 

0i 

1 

0 

-tz/2 

0 

0i 

2 

0 

n/2 

d2 

02 

3 

0 

0 

d3 

0 

Rotation  Matrices: 


*i° 


0 


0  -5; 

0  Cj 
-1  0 


0 


0  s2 
0  -c2 
1  0 


*3 


1  0  0 
0  1  0 
0  0  1 


(A-32) 


Constants: 


As=m2r2+m3d2 

Bs=:m3r3  (note  r3  is  not  constant) 
Es=m3r32 
F  s— LyjdTsyy 
Gs-l2xx+I3xx 
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Hs  I3zz”^l2zz 
Ks=m3a22+m2r22 
Os=m]ri+(m2+m3)di 

Position  Vector  to  CG 


-Asst  +  Bscts2 
Asc,  +  Bss]s2 
O  +  Bsc2 


(A-33) 


(A-34) 


Rigid  Inertia  Forces 


Bf 


~AsCi~BsS\S2  BAC2 

~Assl  +  Bscis2  Bss,c2  m2sxs2 
0  -  Bss2  m2 c2 


Bf  =  ~mlS2r2 


(A-35) 


Rigid  Inertia  Interaction  Torques 


firo(M)  =  (ff  ,  ~  ~  Gs)C\CiS2  +  BAa\C\S2  +  a2SiC2)  ~  AsU\Sl 

Bro(l<2)  =  -(Es  +  Fs)s]  +  Bs{alsic2  -  a2c,s2 ) 

Br0(l,3)  =  m3(als,s2  +  a2c,c2 ) 

Bia(2,\)  =  (H  s  -  E  5  -  G  S)s]s2c2  +  Bs(alsls2  -  a2c,c2)  +  Asalcl 
Bro(2’2)  =  (Es  +  FJc,  -  B  s  (a,CjC2  +  a2s{s2) 

£ro(2,3)  =  -  m  }  (a  tc,s2  -  a2s,c2 ) 


Br0(3,l)  =  Es  +  +  Gs  +  Ks  +  (H,  -  Es  -  G  s)c2 

5ro(3>  2)  =  -  B sa 2c2 

Br0(  3,3)  =  -m3a2s2  (A -36) 


Rigid  Nonlinear  Coriolis  Torques 

NRT0(l,l)=-cos(thl)*(2*m3*rc3A2*cos(th2)A2+2*I3xx*cos(th2)A2-2*I3zz*cos(th2)A2+2*I2xx*cos(th2)A2 

-2*I2zz*cos(th2)A2+I3zz+I3yy-I3xx+I2zz+I2yy-I2xx-2*m3*cos(th2)*rc3*al) 

NRt0(2,1)=  -sin(thl)*(2*m3*rc3A2*cos(th2)A2+2*I3xxNos(th2)A2-2*I3zzNos(th2)A2+2*I2xx*cos(th2)A2 
-2*I2zz*cos(th2)A2+I3zz-t-I3yy-I3xx+I2zz+I2yy-I2xx-2*m3*cos(th2)*rc3*al) 

NRt0(3)l)=2*sin(th2)*cos(th2)*(m3*rc3A2+I3xx-I3zz+I2xx-I2zz) 

NRto(  1 ,2)=NRt0(  1,3)=  NRx0(2,2)=NRt0(2,3)=  NR,0(3,2)=  NRt0(3,3)=0  (A-37) 
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Rigid  Nonlinear  Centrifugal  Torques 

NCx0(l,l)=  cos(thl)*m3*cos(th2)*rc3*a2-cos(thl)*m3*a2*al-  cos(thl)*m2*rc2*al 

+sin(thl)*sin(th2)*m3*rc3A2*cos(th2)+sin(thl)*sin(th2)*I3xx*cos(th2)-  sin(thl)*sin(th2)*I3zz*cos(th2) 
+sin(thl)*sin(th2)*I2xx*cos(th2)-sin(thl)*sin(th2)*I2zz*cos(th2)-sin(thl)*m3*rc3*al*sin(th2) 

NCx0(l,2)=  -m3*rc3*(cos(thl)*cos(th2)*a2+sin(thl)*al*sin(th2)) 

NCx0(2,l)=  sin(thl)*m3*cos(th2)*rc3*a2-sin(thl)*m3*a2*al-sin(thl)*m2*rc2*al 
-  cos(thl)*m3*rc3A2*cos(th2)*sin(th2)-cos(thl)*cos(th2)*I3xx*sin(th2)+cos(thl)*sin(th2)*I3zz*cos(th2) 
-cos(thl)*cos(th2)*I2xx*sin(th2)+cos(thl)*sin(th2)*I2zz*cos(th2)+cos(thl)*m3*rc3*al*sin(th2) 

NCx0(2,2)=  -m3*rc3*(sin(thl)*cos(th2)*a2-cos(thl)*al*sin(th2)) 

NCx0(3,2)=  m3*rc3*a2*sin(th2) 

NCx0(l,3)=  NCt0(2,3)=  NCxo(3,1)=  NCxO(3,3)=0  (A-38) 


Rigid  Joint  Torques 


E+G+H+Il)y+(D-E-G)cl  ~Baf2 


-Ba^  E+F  0 

-"¥¥2  0  "h 


(A-39) 
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A.3  Three  Degree  of  Freedom  Wrist 


The  third  configuration  chosen  was  a  wrist  configuration  due  to  its  common  use 
in  combination  with  other  robots  and  availability  for  use  in  conjunction  with  the 
anthropomorphic  robot  on  the  experimental  testbed. 


Table  A-3 


Denavit-Hartenberg  Parameters  for  Wrist 


Link 

aj 

as 

di 

0i 

3 

0 

nil 

d3 

03 

4 

0 

-nil 

d4 

04 

5 

0 

nil 

d5 

05 

6 

0 

0 

d6 

06 

Rotation  Matrices 


cos(#4)  0  -sin(04) 
sin(#4)  0  cos(04) 


0 


-1 


0 


cos(06)  -sin(06)  O' 
sin(06)  cos(06)  0 


0 

cos(#5) 
sin(05 ) 
0 


0  1 
0  sin(#5 ) 
0  -cos(05) 
1  0 


(A-40) 


Constants 


Aw=m5r5+(d5+r6)m6 
Ew=m6r62 
Jw  I6yy“l6xx 
Kw=m5r5  +m6d52 
Lw_  I5xx"l5zz+l6yy'l6zz  “^Hlyl'yds 
Mw  I5yy+l6xx+2m6r6d5 

Nw  Lvy^  L  xx+Lyy+2 1T1  ()ly  d  5 


(A-41) 
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Position  Vector  to  CG 


rCG  = 


m6  cos(#4)sin(#5) 
m6  sin(04)sin(05) 
m3  (cos(6s  ) — r3 ) 


Rigid  Inertia  Forces 


Am 

Am  Avs4c5 


o 


~AWSS 


0 

0 

0 


Rigid  Inertia  Interaction  Torques 


B,o  OA)  =  ~ss  { (Kw  +  Ew  +  Lw)cAc5  +  AwdAcA  +  Jwc6(sas6  —  c4c5c6)J 
■®ro(I>2)  —  —(Mw  +  Ew  +  Lw)sa  —  AwdAsAc5  —  Jwc6(sAc6  +  c4c5s6) 

■®roO»^)  =  ^6zzC4S5 

Bvq(2,Y)  =  Ss  (-^iv  ■*"  K  +  ^w)S4C5  ~  Aw^4S4  ■*"  JwCAC4S6  ~  54C5Cs)} 

Bt0(2,2)  =  (Mw  +  EW  +  Lw)ca  +  AJacac5  +  Jwc6(cAc6  -  s4c556) 
Br0{2,3)  —  I6zzsas5 

Bt0(  3,D  =  s25(Kw  +  Ew  +  LJ  -  jy5c26 
Bfo  (3, 2)  =  Jws5s6c6 
Bi0(3,3)  =  I6zzc5 


(A-42) 


(A-43) 


(A-44) 
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A.4  Six  Degree  of  Freedom  Anthropomorphic  Robot  with  Wrist 

Rotation  Matrices 


cos(^) 

0  sin(0,) 

K  = 

sin(0,) 

0  -cos(0,) 

0 

1  0 

cos(02) 

-sin(02)  0 

r12  = 

sin(02) 

cos(#2)  0 

o 

0  l. 

cos(03) 

-sin(#3)  0 

Rl  = 

sin(03) 

cos(03)  0 

0 

0  l. 

0 

-1  0 

K  = 

cos(04) 

0  sin(04) 

-sin(6>4) 

0  cos(<94) 

l~cos(05)  0  sin(05)  1 

r;  = 

sin(05) 

0  -cos(#5) 

_0 

1  0 

~cos(06) 

-sin(06)  O' 

*6  = 

sin(06) 

cos(#6)  0 

0 

0  1 

Constants 

Mmv  =  m2r2  +  (iHj  +  m4+m5  +  m6)a2 

Naw  =  m3r3  +  (m4  +m5+  m6)a3  +  m4r4  +  (m5  +  m6)d4 

Oaw  =  m5r5+m6d5+m6r6 

Paw  =  moro  +  ^o(wi  +  m2+m3+m4+m5  +  m6 )  +  dr(m2  +  m3  +  m4  +  m5 
M Taw  =  m0  +  ml  +  m2  +  m3  +  m4  +  ms  +  m6 


(A-45) 


+  m6)  +  mlrl 
(A-46) 
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Position  Vector  To  CG 


C|(MwC2  +^jwC23  +  ^awC5C23  ^awC4S5S23)  ^\S4S5 

S.(%2  +Na,C23  +OmC5Cs  -CLW23)  +  0;1V5 

Pm  +K/2  +N^23  +0^ B  +0^% 


(A47) 


Rigid  Interaction  Forces 


Bf- 


~q {Mc2  + (N+  Cte5 )  + q {M:2  +c23(A^+(^5)“Qs23c455} “Qs,1^5 


0 


-q{M2  +5,23(A^+Cfc5)+Cb23c45,5}  -q{M2  +1s,23(iV+Ob5)+6fc23C41s,5}  c2(M +N  JrOc5)-OsvcAs5 

— q  ( JV + Oc5 ) + c455  }  —  q  {523(^4- Cfc5)+ (^23^4^5}  ^23^4^5 

Cfc5(w4  -5,cj  %¥(+%) 

— 0{Cj  S^C^C^)  ■+*  0{— ^(CjjSj  +sac4c5)+c,,s4c5} 

0  0 


OcgS^s^ 


(XfiziFiPs  ^5^23) 

0 

(A-48) 


Rigid  Interaction  Coriolis  Forces 


ML=—* 

V 

\W^2+%(K+0<J:5)+0J:VC4SS}  -^{Mo^2+S23(Nm+Oj:5)+OmfaC4S5}  0 

2s\  {s^  {Nm + Olvp5 ) + Q.^c^ }  -  2c,  {i’3  ,(Nm  +  Oiric5 ) + Ocr_  }  0 

$5  ^  Vl  }  0 

2CL{‘Sl(%+523C4^)-W5}  0 

0  0  0 


{"^23  (^iv  +  On  C5  ) + } 

%Ws 

■^K^+Ws) 

0 

%1W5 

0 

-^${-^4+%} 


tasi 

(-23<Vs  } 

0 

^QufS'l<:23-SVS5 

{  ^*23^5  +CaC4Cj} 

0 


"  2{-?23  OXnt- +  Gr.vC5  )  +Q»PziC4p5  } 

2Qn%A% 

-^fe+Wsl 

0 

2DmipBs4s5 

~^Qw^PzS5  +5BC4C5} 

0 

~2QmPbS4C5 


(A-49) 
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Rigid  Interaction  Centrifugal  Forces 


AW  =—  *  Ws  {-Mj^  -eB(AL  +0mP5)+0(J23}+0aiAs4ss 

Mrm 

A C/U)  =-i-  {-M^  -^(AL+CUJ+O^J 

Km 

Wf{\Z)=^\{^{NM+Oj:5)+Ojhf<s5} 

AC/1,4)  =—- *{°»(W4  +W} 

A4v 

AC^O,  5)  =T7-*{<^Jq(W5  -%]+ Ws} 

MTa, 

Aq.(l,6)=0 

A^y  (2, 1)  —  *  fa  [— ^afl  _ C23  (A^v +QmPxC4S5  ] — 

A4w 

MTm 

^(2.3)=Tj-*KW,+a,A)+O^A) 

'jfaw 

AC/(2,4)  =-7-  *  {Q.AfaV*  -^4) 

MTm 

NOfQ  5)  =-J-  *{0J$(w5  -%)-«]} 

Km 

NCf(Z6)=0 

7\C/(3,1)=0 

AC/iZI—'HM^+^^+O^+O^CA} 

Mt« 

Ac/a3)=Tr*(-%(iv„+Q,A)+a^w) 

ac/64)=i r*Hiw,> 

MTm 

AC/3, 5)  =-J-  *HQ/yS,23  +Ws) 

(A-50) 
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